1 /*===================================================================================================*/

2 /*PROGRAM INTRODUCTION
3 Motor speed control by PID controller. Both channel A and B of the encoder are connected to
4 interrupt 0(pin2) and interrupt 1 (pin3) to calculate speed. The formula is as bellowing:
5 rotationSpeed=60000*positionDifference/(PulsePerRevolution*DECODE_NUMBER*timeDifference*GEAR_RATIO).
6 Constants and parameters are denoted by capital letters. E.g.: HIGH_LIMIT (=100.0).
7 HHManh, Mechatronics, AIT Thailand.*/
8 /*===================================================================================================*/
9
10 /*CONSTANTS AND GLOBAL VARIABLE DECLARATION*/
11 const int ENCODER_PINA = 2;//pin for encoder's channel A.
12 const int ENCODER_PINB = 3;//pin for encoder's channel B.
13 const int DIR1 = 6;//direction 1, DIR1=0,DIR=1, CW rotation.
14 const int DIR2 = 7;//direction 2, DIR1=0,DIR=1, CW rotation.
15 const int BUTTON_PIN = 8;//mode: FALSE = AUTO, TRUE = MAN.
16 const int LED_PIN = 9;//LED connection.
17 const int ENA = 11;//PWM output pin connecting to the driver.
18 const int PPR = 12;//number of pulses per revolution by encoder.
19 const int GEAR_RATIO = 64;//gear ration from motor to main shaft.
20 const int DECODE_NUMBER = 4;//depends on using interrupt (see encoder section).
21 const boolean AUTO = true;//automatic (PID) mode.
22 const boolean MANUAL = false;//manual mode (speed control by manual input value.
23 const double HIGH_LIMIT = 100.0;//high limit.
24 const double LOW_LIMIT = 0.0;//low limit.
25 const int DEBOUNCE_TIME = 50;//debounce time in ms.
26 /*variables used for PID*/
27 double rotationSpeed = 0.0;//rotation speed in RPM.
28 double settingSpeed = 0;//setting speed at manual mode.
29 double controlVariable;//control variable.
30 double GAIN = 0.015;//proportional gain.
31 double TI = 2000;//intergral time in ms.
32 double TD = 0;//derivative time in ms.
33 double CYCLE = 100;//PID processing interval.
34 double manualValue = 0;//manual value.
35 boolean operationMode = false;//telling the operation mode. Manual = true, Auto = false.
36 boolean lastOperationMode = false;
37 //vars in Interrupt Subroutine need to be volatile.
38 volatile long currentPosition = 0;//number of pulses at time of calculate
39 int dutyCycleMap = 0;//duty cycle value after mapping 0 - 255.
40 long lastPosition = 0;//last number of pulses used in speed calculate.
41 long positionDifference = 0;//difference in pulse number (used in speed calculate).
42 unsigned long timeDifference = 0;//difference in time used in speed calculate.
43 unsigned long lastDisplayTime;//interval time var in ms for display.
44 unsigned long lastSpeedTime;//interval time var in ms for RPM calculate.
45 /*Following vars for preventing button bounce from press.*/
46 unsigned long lastDebounceTime;
47 int buttonState, lastButtonState, bitToggle;
48 /*===================================================================================================*/
49
50 void setup()
51 {
52 TCCR2B = TCCR2B & 0b11111000 | 0x01;//PWM frequency changed to 31250Hz
53 pinMode(BUTTON_PIN, INPUT);//set pin BUTTON_PIN as input.
54 pinMode(ENCODER_PINA, INPUT);//set pin ENCODER_PINA as input.
55 pinMode(ENCODER_PINB, INPUT);//set pin ENCODER_PINB as input.
56 pinMode(LED_PIN, OUTPUT);//set pin LED_PIN as output.
57 pinMode(DIR1, OUTPUT);//set pin DIR1 as output.
58 pinMode(DIR2, OUTPUT);//set pin DIR2 as output.
59 pinMode(ENA, OUTPUT);//set pint ENA as output
60 //digitalWrite(DIR1, LOW);//by writting pin DIR1 to LOW and pin DIR2 to HIGH. CW direction is set.
61 //digitalWrite(DIR2, HIGH);
62 attachInterrupt(0, doEncoderA, CHANGE);//set up ISR for interrupt 0 - pin 2.
63 attachInterrupt(1, doEncoderB, CHANGE);//set up ISR for interrupt 1 - pin 3.
64 Serial.begin(9600);//serial speed 9600kb/s.
65 //Serial.println("OPERATING INFORMATION");
66 Serial.println("Q + ENTER for PID parameter display or others for parameter change!");
 67 Serial.println("P + ENTER => GAIN = GAIN + 0.001, p + ENTER => GAIN = GAIN - 0.001");
68 Serial.println("I + ENTER => TI = TI + 10 (ms), i + ENTER => TI = TI - 10 (ms)");
69 Serial.println("D + ENTER => TD = TD + 10 (ms), d + ENTER => TD = TD - 10 (ms)");
70 Serial.println("AUTOMATIC mode: Press 'setting-speed' + C => speed setpoint");
71 Serial.println("MANUAL mode: Set direction and duty-cycle (0 - 100) to rotate the motor: ");
72 Serial.println("(R+ENTER => CW direction, L+ENTER => CCW direction, 'duty-cycle'+C+ENTER => Speed");
73 Serial.print("S+ENTER => Slow stop, B+ENTER => Fast stop)"); Serial.println(" G+ENTER => information");
74 Serial.println("Ready in MANUAL mode!");
75 }
76
77 /*===================================================================================================*/
78
79 void loop()
80 {
81
82 /*This part is for preventing bounce from press the mode button. The toggle button is used to
83 change the mode. LED ON or OFF by pressing the button. LED ON = AUTOMATIC mode, LED OFF = MANUAL MODE.*/
84
85 int readingButton = digitalRead(BUTTON_PIN);
86 if (readingButton != lastButtonState)
87 {
88 lastDebounceTime = millis();
89 }
90 if ((millis()-lastDebounceTime) > DEBOUNCE_TIME)
91 {
92 if (readingButton != buttonState)
93 {
94 buttonState = readingButton;
95 if (buttonState == HIGH)
96 {
97 bitToggle = !bitToggle;
98 }
99 }
100 }
101 lastButtonState = readingButton;
102 digitalWrite(LED_PIN, bitToggle);//Writing to LED.
103 operationMode = digitalRead(LED_PIN);//operation mode is resulted from LED status.
104
105 /*---------------------------------------------------------------------------------------------------
106 Display setting speed, control variale, direction */
107
108 if (millis() - lastDisplayTime > 2000)
109 {
110 if (currentPosition != lastPosition)
111 {
112 if (operationMode == MANUAL)
113 {
114 Serial.print("Setting manual_value = "); Serial.print(manualValue); Serial.print(" %");
115 Serial.print(". Control_variable = "); Serial.print(controlVariable); Serial.println(" %");
116 Serial.print("Real speed = "); Serial.print(rotationSpeed);Serial.print(" RPM");
117 Serial.print(". Followed setting_speed = "); Serial.print (settingSpeed ); Serial.println("
RPM");
118 Serial.println("------------------------------------------------------------");
119 }
120 if (operationMode == AUTO)
121 {
122 Serial.print("Setting_speed = "); Serial.print (settingSpeed ); Serial.print(" RPM");
123 Serial.print(". Real speed = "); Serial.print(rotationSpeed);Serial.println(" RPM");
124 Serial.print("Control_variable = "); Serial.print(controlVariable); Serial.print(" %");
125 Serial.print(". Followed manual_value = "); Serial.print(manualValue); Serial.println(" %"
);
126 Serial.println("------------------------------------------------------------");
127 }
128 lastDisplayTime = millis();
129 }
130 }//if
131
132 /*---------------------------------------------------------------------------------------------------
133 RPM speed calculation formula:
134 rotationSpeed = 60000.0*positionDifference/(PPR*DECODE_NUMBER*GEAR_RATIO*timeDifference)
135 PPR = 12, decodeNumnber = 4, GEARratio = 64, 60000/(12*4*64) = 19.53125.*/
136 if (millis() - lastSpeedTime > 50)
137 {
138 timeDifference = millis()-lastSpeedTime;
139 positionDifference = currentPosition - lastPosition;
140 if (positionDifference < 0)
141 {
142 positionDifference = -positionDifference;
143 }
144 rotationSpeed = 19.53125 * positionDifference/timeDifference;
145 lastSpeedTime = millis(); //update speed calculation time.
146 lastPosition = currentPosition;}//update number of counts for position.
147
148 /*---------------------------------------------------------------------------------------------------
149 Parameters and operation mode display */
150 if ((lastOperationMode == MANUAL) && (operationMode == AUTO))
151 {
152 Serial.println("System is changed to AUTOMATIC mode!");
153 Serial.println("Press 'setting-speed'+C+ENTER => speed setpoint!");
154 }
155 //Serial.println("==================================================================");}
156 if ((lastOperationMode == AUTO) && (operationMode == MANUAL))
157 {
158 Serial.println("System is changed to MANUAL mode!");
159 Serial.println("(S+ENTER => Slow stop, B+ENTER => Fast stop)");
160 }
161 //Serial.println("==================================================================");}
162 lastOperationMode = operationMode;
163
164 /*---------------------------------------------------------------------------------------------------
165 Interface for manual mode */
166 if (Serial.available())
167 {
168 byte ch = Serial.read();
169 if (operationMode == MANUAL)
170 {
171 static double dutyCycle;
172 switch(ch)
173 {
174 case 'R' : digitalWrite(DIR1, LOW); digitalWrite(DIR2, HIGH);
175 Serial.println("CW rotation direction is set!");
176 //Serial.println("=============================");
177 Serial.println("Set duty-cycle (0 - 100): 'duty-cyle'+C+ENTER ");
178 break;
179
180 case 'L' : digitalWrite(DIR1, HIGH); digitalWrite(DIR2, LOW);
181 Serial.println("CCW rotation direction is set!");
182 //Serial.println("=============================");
183 Serial.println("Set duty-cycle (0 - 100): 'duty-cyle'+C+ENTER ");
184 break;
185
186 case '0'...'9':
187 dutyCycle = dutyCycle * 10 + ch - '0';
188 break;
189
190 case 'C' :
191 if ((dutyCycle >= 0) && (dutyCycle <= 100))
192 {
193 manualValue = dutyCycle;
194 Serial.print("Duty-cycle is set at: "); Serial.print(dutyCycle); Serial.println(" % !"
);
195 Serial.print("dutyCycleMap = "); Serial.print(dutyCycle); Serial.println(" / 255");
196 Serial.println("==============================");
197 }
198 else
199 {
200 Serial.println ("Wrong setting, enter value from 0 - 100!");
201 Serial.println("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx");
202 }
203 dutyCycle = 0;
204 break;
205
206 case 'S' :
207 digitalWrite(DIR1, LOW);
208 digitalWrite(DIR2, LOW);
209 manualValue = 0;
210 Serial.println("Slow stop is activated!");
211 break;
212
213 case 'B' :
214 digitalWrite(DIR1, HIGH);
215 digitalWrite(DIR2, HIGH);
216 manualValue = 0;
217 Serial.println("Fast stop is activated!");
218 break;
219 }//Switch-case
220 }//if
221
222 /*---------------------------------------------------------------------------------------------------
223 Interface for automatic mode */
224 else
225 {
226 static double sendSetPoint = 0;
227 switch(ch)
228 {
229 case '0'...'9':
230 sendSetPoint = sendSetPoint * 10 + ch - '0';
231 break;
232
233 case 'C' :
234 if ((sendSetPoint >= 0) && (sendSetPoint <= 130))
235 {
236 settingSpeed = sendSetPoint;
237 Serial.print("Setpoint speed is : "); Serial.print(settingSpeed);
238 Serial.println(" RPM");
239 }
240 else
241 {
242 Serial.println ("Wrong setting, enter value from 0 - 130 only!");
243 Serial.println("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx");}
244 sendSetPoint = 0;
245 break;
246 }//switch-case
247 }//else
248
249 /*This below section is for PID parameter modification and display. At any time, pressing Q will
250 display PID parameters. Capital letters for increase and lower case ones for decrease of parameters.
251 After any change, 03 parameters are shown.*/
252
253 switch(ch)
254 {
255 case 'Q' :
256 Serial.print("GAIN = "); Serial.println(GAIN,4);//print 04 digits after decimal point.
257 Serial.print("TI = "); Serial.print(TI); Serial.println(" milliseconds");
258 Serial.print("TD = "); Serial.print(TD); Serial.println(" milliseconds");
259 Serial.println("========================================================");
260 break;
261
262 case 'P' :
263 GAIN += 0.001; //GAIN = GAIN + ...
264 Serial.print("GAIN is changed to: ");
265 Serial.println(GAIN,4);//print 04 digits after decimal point.
266 Serial.println("========================================================");
267 break;
268
269 case 'p' : GAIN -= 0.001;
270 Serial.print("GAIN is changed to: ");
271 Serial.println(GAIN,4);//print 04 digits after decimal point.
272 Serial.println("========================================================");
273 break;
274
275 case 'I' : TI += 10;
276 Serial.print("TI is changed to: ");
277 Serial.print(TI);
278 Serial.println(" milliseconds");
279 Serial.println("========================================================");
280 break;
281
282 case 'i' : TI -= 10;
283 Serial.print("TI is changed to: ");
284 Serial.print(TI);
285 Serial.println(" milliseconds");
286 Serial.println("========================================================");
287 break;
288
289 case 'D' : TD += 10;
290 Serial.print("TD is changed to: ");
291 Serial.print(TD); Serial.println(" milliseconds");
292 Serial.println("========================================================");break;
293 case 'd' : TD -= 10;
294 Serial.print("TD is changed to: "); Serial.print(TD); Serial.println(" milliseconds");
295 Serial.println("========================================================");
296 break;
297 }
298 }//main if (166)
299
300 //case 'G' :
301 //Serial.println("OPERATING INFORMATION");
302 //Serial.println("Press Q + ENTER for PID parameter display or others for parameter change!");
303 //Serial.println("P + ENTER => GAIN = GAIN + 0.001, p + ENTER => GAIN = GAIN - 0.001");
304 //Serial.println("I + ENTER => TI = TI + 10 (ms), i + ENTER => TI = TI - 10 (ms)");
305 //Serial.println("D + ENTER => TD = TD + 10 (ms), d + ENTER => TD = TD - 10 (ms)");
306 //Serial.println("AUTOMATIC mode: Press 'setting-speed' + C => speed setpoint");
307 //Serial.println("MANUAL mode: Set direction and duty-cycle (0 - 100) to rotate the motor: ");
308 //Serial.println("(R + ENTER => CW direction, L + ENTER => CCW direction, 'duty-cycle' + C => Speed");
309 //Serial.println("S + ENTER => Slow stop, B + ENTER => Fast stop)"); break;}}
310
311 /*---------------------------------------------------------------------------------------------------
312 PID function is process at interval of CYCLE*/
313
314 unsigned long lastPIDTime;//to store the real-time when previous execution is done.
315 if (millis()- lastPIDTime > CYCLE)
316 {
317 PID();
318 lastPIDTime = millis();
319 }
320 /*---------------------------------------------------------------------------------------------------
321 Below for mapping to digital value (0-255) and write to PWM output.*/
322
323 dutyCycleMap = map(controlVariable,0,100,0,255);//Mapping manipulated var to digital value.
324 analogWrite(ENA, dutyCycleMap);}//Write pulse train to PWM output pin
325
326 //END OF LOOP
327 /*===================================================================================================*/
328
329 /*PID ALGORITHM
330 lmn is output of the PID controller and consists of 03 portions: proportional, integral, & derivative.
331 lmn (called loop-manipulated variable): lmn = lmnP + lmnI + lmnD.
332 lmn = GAIN*(ER + (1/TI)*integral(ER*dt) + TD*d(ER)/dt) (dt = CYCLE: processing interval).
333 lmnP = GAIN*ER = GAIN * (setPoint - processVariable). setPoint & processVariable (in percent 0 - 100).
334 lmnI = GAIN*(1/TI)*integral(ER*dt) <=> d(lmnI) = GAIN*(1/TI)*ER*dt = lmnP*(1/TI) * CYCLE
335 <=> lmnI-lastLmnI = lmnP*(1/TI) * CYCLE <=> lmnI = lastLmnI + lmnP*CYCLE/TI.
336 lmnD = GAIN*TD*d(ER)/dt = TD* d(GAIN*ER)/dt = TD*d(lmnP)/dt = TD * (lmnP - lastLmnP)/CYCLE.
337 lmnD = (TD/CYCLE)*(lmnP - lastLmnP)
338 Bumpless manual-to-auto switch: in manual mode, lmn = manualValue, settingSpeed = rotationSpeed
339 then back-calculate lmnI.
340 Bumpless auto-to-manual switch: in auto mode manualValue = ControlVariable.
341 Preventing integral-windup: when lmn is out of limit we set lmn to limit and back calculate lmnI.*/
342
343 void PID()
344 {
345 double processVariable;//speed in percentage.
346 double setPoint;//setspeed in percentage.
347 double lmn;//loop manipulated variable
348 double lmnP;//proportional portion.
349 double lmnD;//derivative portion.
350 double integralDiff;//Integral difference
351 static double lmnI;//integral portion. Static vars for remembering last value.
352 static double lastLmnP;//last value of lmnP.
353 processVariable = map(rotationSpeed,0,130,0,100);
354 setPoint = map(settingSpeed, 0,130,0,100);
355 //Proportional portion calculation
356 lmnP = GAIN * (setPoint - processVariable);
357 //at auto mode
358 if (operationMode == AUTO)
359 {
360 //Integral portion calculation.
361 if (( (integralDiff > 0) && (lmn > HIGH_LIMIT) ) || ( (integralDiff < 0) && (lmn < LOW_LIMIT) ) )
362 {
363 integralDiff = 0.0;
364 }
365 else
366 {
367 integralDiff = lmnP * CYCLE/TI;
368 }
369 lmnI += integralDiff;
370
371 //Derivative portion calculation.
372 lmnD = (TD/CYCLE) * (lmnP - lastLmnP);
373 lastLmnP = lmnP;//update lmnP
374 lmn = lmnP + lmnI + lmnD;//sum of portions
375
376 //Safety limit
377 if ((lmn > HIGH_LIMIT) && (lmnI > HIGH_LIMIT))
378 {
379 lmnI = lmnI - (lmn - HIGH_LIMIT);
380 }
381 if ((lmn < LOW_LIMIT) && (lmnI < LOW_LIMIT))
382 {
383 lmnI = lmnI + (LOW_LIMIT - lmn);
384 }
385 if (lmn > HIGH_LIMIT)
386 {
387 lmn = HIGH_LIMIT;
388 }
389 if (lmn < LOW_LIMIT)
390 {
391 lmn = LOW_LIMIT;
392 }
393 manualValue = lmn;
394 }//if
395 //At manual mode
396 else
397 {
398 lmn = manualValue; lmnI = lmn - lmnP;settingSpeed = rotationSpeed;
399 }
400 controlVariable = lmn;
401 }
402
403 /*===================================================================================================*/
404
405 /*INTERRUPT SUBROUTINE CHANNEL A
406 When an interrupt on channel A occurs then: CW rotation: A != B, CCW rotation: A = B.8*/
407 void doEncoderA()
408 {
409 if (digitalRead(ENCODER_PINA) != digitalRead(ENCODER_PINB))
410 {
411 currentPosition++;
412 }
413 else
414 {
415 currentPosition--;
416 }
417 }
418 /*===================================================================================================*/
419
420 /*INTERRUPT SUBROUTINE CHANNEL B
421 When an interrupt on channel B occurs then: CW rotation: A = B, CCW rotation: A != B.*/
422 void doEncoderB()
423 {
424 if (digitalRead(ENCODER_PINA) == digitalRead(ENCODER_PINB))
425 {
426 currentPosition++;
427 }
428 else
429 {
430 currentPosition--;
431 }
432 }
433 /*END OF PROGRAM
434 ====================================================================================================*/