Components and supplies
Jumper wires (generic)
Photoelectric Speed Sensor HC-020K
Breadboard (generic)
DC motor (generic)
Arduino Due
Slide Switch
Rotary potentiometer (generic)
Resistor 4.75k ohm
Makeblock Me TFT LCD
General Purpose Transistor PNP
Apps and platforms
Arduino IDE
Project description
Code
Code without Graphical Touch-Ups
arduino
1/* ############################################### 2 I/O Assignments 3############################################### */ 4int _chSpeedSet = A0, // Speed setpoint 5 _chKp = A1, // Proportional coefficient reading for PID controller 6 _chKi = A2, // Integral coefficient reading for PID controller 7 _chKd = A3, // Derivative coefficient reading for PID controller 8 _chMotorCmdCCW = 3, // PWM output to motor for counter-clockwise turn 9 _chMotorCmdCW = 2, // PWM output to motor for clockwise turn 10 _chSpeedRead = 24, // Speed reading 11 _chDirection = 25; // Direction selector reading 12 13/* ############################################### 14 Other Constants 15############################################### */ 16#define _minRPM 0 // Minimum RPM to initiate direction changing 17#define _maxRPM 6000 // Maximum RPM limit 18#define _DiscSlots 20 // Qty of slots on Index Disc 19 20/* ############################################### 21 Global Variables 22############################################### */ 23boolean Direction, prevDirection; 24float RPM=0.0, RPMset=0.0, OutputRPM=0.0, 25 Kp=0.0, Ki=0.0, Kd=0.0, 26 Kpmax=2.0, Kimax=1.0, Kdmax=1.0, 27 E=0.0, Eprev=0.0, dT=1.0; 28 29/* ############################################### 30 readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed) 31 Frequency Reading Function 32 Input Parameters: 33 (int) _DI_FrequencyCounter_Pin : Digital pin to be read 34 (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1) 35 36 Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted. 37 It cannot be 0(zero), negative values or a value greater than 10. 38 When _ReadingSpeed changed, 1 second shall be divided by this value to calculate 39 required counting duration. For example; 40 - _ReadingSpeed = 0.1 -> input shall be counted during 10 seconds (=1/0.1) 41 - _ReadingSpeed = 0.5 -> input shall be counted during 2 seconds (=1/0.5) 42 - _ReadingSpeed = 2.0 -> input shall be counted during 0.5 seconds (=1/2) 43 - _ReadingSpeed = 4.0 -> input shall be counted during 0.25 seconds (=1/4) 44 Importantly note that, increasing of _ReadingSpeed is a disadvantage especially 45 on lower frequencies (generally below 100 Hz) since counting error increases 46 up to 20%~40% by decreasing frequency. 47############################################### */ 48 49int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed) 50{ 51 pinMode(_DI_FrequencyCounter_Pin,INPUT); 52 byte _DigitalRead, _DigitalRead_Previous = 0; 53 unsigned long _Time = 0, _Time_Init; 54 float _Frequency = 0; 55 if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); 56 else 57 { 58 _Time_Init = micros(); 59 do 60 { 61 _DigitalRead = digitalRead(_DI_FrequencyCounter_Pin); 62 if ( (_DigitalRead_Previous==1) && (_DigitalRead==0) ) _Frequency++; 63 _DigitalRead_Previous = _DigitalRead; 64 _Time = micros(); 65 } 66 while ( _Time < (_Time_Init + (1000000/_ReadingSpeed)) ); 67 } 68 return (_ReadingSpeed * _Frequency); 69} 70/* ########### End of readFrequency() ########### */ 71/* ############################################## */ 72 73/* ############################################### 74 controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) 75 PID Controller Function 76 Input Parameters: 77 (float) RangeMin: Minimum limit for output 78 (float) RangeMax: Maximum limit for output 79 (float) _E_____: Current error signal 80 (float) _Eprev : Previous error signal 81 (float) _dT____: Time difference as seconds 82 (float) _Kp____: Proportional coefficient 83 (float) _Ki____: Integral coefficient 84 (float) _Kp____: Derivative coefficient 85 Adjustment procedure: 86 1. Set Kp=0, Ki=0, Kd=0. 87 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note 88 critical gain Kc = Kp. 89 3. Adjust final coefficients as follows. 90 for P-control only : Kp = 0.50*Kc 91 for PI-control only : Kp = 0.45*Kc, Ki = 1.2/Pc 92 for PID-control : Kp = 0.60*Kc, Ki = 2.0/Pc, Kd=Pc/8 93 4. Fine tuning could be done by slightly changing each coefficient. 94############################################### */ 95 96float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd) 97{ 98 float P, I, D; 99 /* 100 Base Formula: U = _Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); 101 */ 102 P = _Kp * _E; /* Proportional Component */ 103 I = _Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ 104 D = _Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ 105 return (P+I+D); 106} 107/* ########### End of controllerPID() ########### */ 108/* ############################################## */ 109 110/* ############################################### 111 Setup 112############################################### */ 113 114void setup() 115{ 116 analogReadResolution(12); 117 pinMode(_chDirection,INPUT); // Direction selector reading 118 pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn 119 pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn 120 // Initial killing the PWM outputs to motor 121 analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); 122 // Initial reading for direction selection 123 Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW 124 prevDirection=Direction; 125} 126 127 128/* ############################################### 129 Loop 130############################################### */ 131 132void loop() 133{ 134 // Initialization Time: Necessary for PID controller. 135 int InitTime = micros(); 136 137 // Reading Inputs 138 /* Controller Coefficients */ 139 Kp = Kpmax * (float)analogRead(_chKp) / 4095; 140 Ki = Kimax * (float)analogRead(_chKi) / 4095; 141 Kd = Kdmax * (float)analogRead(_chKd) / 4095; 142 /* Direction Selector */ 143 Direction = digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ 144 /* Actual RPM and RPM Setpoint 145 Note that maximum selectable RPM is 5000. */ 146 RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; 147 RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095; 148 149 // Calculations and Actions 150 /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ 151 E = RPMset - RPM; 152 float cPID = controllerPID(E, Eprev, dT, Kp, Ki, Kd); 153 if ( RPMset == 0 ) OutputRPM = 0; 154 else OutputRPM = OutputRPM + cPID; 155 if ( OutputRPM < _minRPM ) OutputRPM = _minRPM; 156 if ( OutputRPM > _maxRPM ) OutputRPM = _maxRPM; 157 158 /* Changing Direction when inverted */ 159 if ( Direction != prevDirection ) 160 { 161 /* Killing both of the PWM outputs to motor */ 162 analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); 163 /* Wait until motor speed decreases */ 164 do 165 { RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } 166 while ( RPM > _minRPM ); 167 } 168 169 // Writing Outputs 170 if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); 171 else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); 172 173 // Storing Values generated on previous cycle 174 Eprev = E; prevDirection = Direction; 175 176 // Calculating control application cycle time and passed Seconds 177 dT = float ( micros() - InitTime ) / 1000000.0; 178}
Code with Graphical Touch-Ups
arduino
1/* ############################################### 2 Color constants for Makeblock Me TFT LCD 3############################################### */ 4#define _BLACK 0 5#define _RED 1 6#define _GREEN 2 7#define _BLUE 3 8#define _YELLOW 4 9#define _CYAN 5 10#define _PINK 6 11#define _WHITE 7 12 13/* ############################################### 14 I/O Assignments 15############################################### */ 16int _chSpeedSet = A0, // Speed setpoint 17 _chKp = A1, // Proportional coefficient reading for PID controller 18 _chKi = A2, // Integral coefficient reading for PID controller 19 _chKd = A3, // Derivative coefficient reading for PID controller 20 _chMotorCmdCCW = 3, // PWM output to motor for counter-clockwise turn 21 _chMotorCmdCW = 2, // PWM output to motor for clockwise turn 22 _chSpeedRead = 24, // Speed reading 23 _chDirection = 25; // Direction selector reading 24 25/* ############################################### 26 Other Constants 27############################################### */ 28#define _minRPM 0 // Minimum RPM to initiate direction changing 29#define _maxRPM 6000 // Maximum RPM limit 30#define _Tmax 90 // Maximum time limit for graphing 31#define _DiscSlots 20 // Qty of slots on Index Disc 32 33/* ############################################### 34 Global Variables 35############################################### */ 36String Cartesian_SetupDetails; 37boolean Direction, prevDirection; 38// Alarm Settings 39float RALL=500.0, RAL=1000.0, RAH=4000.0, RAHH=4500.0; 40float Seconds=0.0, prevSeconds=0.0, 41 prevRPM=0.0, prevRPMset=0.0, 42 RPM=0.0, RPMset=0.0, OutputRPM=0.0, 43 Kp=0.0, Ki=0.0, Kd=0.0, 44 Kpmax=2.0, Kimax=1.0, Kdmax=1.0, 45 E=0.0, Eprev=0.0, dT=1.0; 46 47/* ############################################### 48 CommandToTFT(TFTCmd) 49 Command Function for Makeblock Me TFT LCD 50 Input Parameters: 51 (String) TFTCmd : Command string 52############################################### */ 53void CommandToTFT(String TFTCmd) 54{ 55 /* Serial Connection used for display */ 56 Serial1.println(TFTCmd); delay(5); 57} 58/* ########### End of CommandToTFT() ########### */ 59/* ############################################# */ 60 61/* ############################################### 62 Cartesian_Setup(Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX, ColorY) 63 Cartesian X-Y Axis Drawing Function for Makeblock Me TFT LCD 64 Input Parameters: 65 (float) Xmin, Xmax, Ymin, Ymax : Axis range values 66 (int) Window_X1, Window_Y1___: Upper-left corner of graph window 67 (int) Window_X2, Window_Y2___: Lower-right corner of graph window 68 (int) MinDashQty_____________: Qty.of dashes on shortest axis 69 (int) ColorB, ColorX, ColorY : Drawing colors for Frame, X-axis and Y-axis 70 Uses external function CommandToTFT(). 71############################################### */ 72String Cartesian_Setup( 73 float Xmin, float Xmax, float Ymin, float Ymax, 74 int Window_X1, int Window_Y1, int Window_X2, int Window_Y2, 75 int MinDashQty, int ColorF, int ColorX, int ColorY 76 ) 77{ 78 /* Screen Limitations */ 79 const int DisplayResolutionX = 319, DisplayResolutionY = 239; 80 /* Limit Title Strings */ 81 String XminTxt; 82 if (abs(Xmin)>=1000000000) XminTxt = "X=" + String (Xmin/1000000000) + "G"; 83 else if (abs(Xmin)>=1000000) XminTxt = "X=" + String (Xmin/1000000) + "M"; 84 else if (abs(Xmin)>=1000) XminTxt = "X=" + String (Xmin/1000) + "K"; 85 else XminTxt = "X=" + String (Xmin); 86 String XmaxTxt; 87 if (abs(Xmax)>=1000000000) XmaxTxt = "X=" + String (Xmax/1000000000) + "G"; 88 else if (abs(Xmax)>=1000000) XmaxTxt = "X=" + String (Xmax/1000000) + "M"; 89 else if (abs(Xmax)>=1000) XmaxTxt = "X=" + String (Xmax/1000) + "K"; 90 else XmaxTxt = "X=" + String (Xmax); 91 String YminTxt; 92 if (abs(Ymin)>=1000000000) YminTxt = "Y=" + String (Ymin/1000000000) + "G"; 93 else if (abs(Ymin)>=1000000) YminTxt = "Y=" + String (Ymin/1000000) + "M"; 94 else if (abs(Ymin)>=1000) YminTxt = "Y=" + String (Ymin/1000) + "K"; 95 else YminTxt = "Y=" + String (Ymin); 96 String YmaxTxt; 97 if (abs(Ymax)>=1000000000) YmaxTxt = "Y=" + String (Ymax/1000000000) + "G"; 98 else if (abs(Ymax)>=1000000) YmaxTxt = "Y=" + String (Ymax/1000000) + "M"; 99 else if (abs(Ymax)>=1000) YmaxTxt = "Y=" + String (Ymax/1000) + "K"; 100 else YmaxTxt = "Y=" + String (Ymax); 101 /* Limits */ int XminPx = Window_X1+1; int XmaxPx = Window_X2-1; 102 int YmaxPx = Window_Y1+1; int YminPx = Window_Y2-1; 103 /* Origin */ int OriginX = XminPx + (int)( (XmaxPx - XminPx) * abs(Xmin) / (abs(Xmax)+abs(Xmin)) ); 104 int OriginY = YmaxPx + (int)( (YminPx - YmaxPx) * abs(Ymax) / (abs(Ymax)+abs(Ymin)) ); 105 /* Frame */ CommandToTFT ( "BOX(" + String(Window_X1) + "," + String(Window_Y1)+ "," + 106 String(Window_X2) + "," + String(Window_Y2)+ "," + 107 String(ColorF) + ");" 108 ); 109 /* X Axis */ CommandToTFT ( "PL(" + String(Window_X1+1) + "," + String(OriginY) + "," + 110 String(Window_X2-1) + "," + String(OriginY) + "," + 111 String(ColorX) + ");" 112 ); 113 /* Y Axis */ CommandToTFT ( "PL(" + String(OriginX) + "," + String(Window_Y1+1) + "," + 114 String(OriginX) + "," + String(Window_Y2-1) + "," + 115 String(ColorY) + ");" 116 ); 117 118 /* 119 Dashing: Minimum amount of dashes is given by "MinDashQty" and will be dashed on the shortest 120 axis-side with respect to origin. 121 On the other sections, dashes to be marked shall be determined by considering ratio to 122 shortest axis-side. 123 */ 124 /* Dashing */ int XlengthLeft = abs(XminPx-OriginX); int XlengthRight = abs(XmaxPx-OriginX); 125 int YlengthLower = abs(YminPx-OriginY); int YlengthUpper = abs(YmaxPx-OriginY); 126 int XlengthLeft_Mod, XlengthRight_Mod, YlengthLower_Mod, YlengthUpper_Mod; 127 if (XlengthLeft<=1) XlengthLeft_Mod=32767; else XlengthLeft_Mod=XlengthLeft; 128 if (XlengthRight<=1) XlengthRight_Mod=32767; else XlengthRight_Mod=XlengthRight; 129 if (YlengthLower<=1) YlengthLower_Mod=32767; else YlengthLower_Mod=YlengthLower; 130 if (YlengthUpper<=1) YlengthUpper_Mod=32767; else YlengthUpper_Mod=YlengthUpper; 131 int MinAxisLength = min ( min (XlengthLeft_Mod,XlengthRight_Mod), min (YlengthLower_Mod,YlengthUpper_Mod) ); 132 int XdashesLeft = MinDashQty * XlengthLeft / MinAxisLength; 133 int XdashesRight = MinDashQty * XlengthRight / MinAxisLength; 134 int YdashesLower = MinDashQty * YlengthLower / MinAxisLength; 135 int YdashesUpper = MinDashQty * YlengthUpper / MinAxisLength; 136 int DashingInterval=2; // Min.interval btw.dashes 137 138 /* X-Dash L */ DashingInterval = (int) (XlengthLeft / XdashesLeft); 139 if (!(DashingInterval<2)) 140 for (int i=OriginX; i>=XminPx; i-=DashingInterval) 141 CommandToTFT ( "PL(" + String(i) + "," + String(OriginY-2) + "," + 142 String(i) + "," + String(OriginY+2) + "," + 143 String(ColorX) + ");" 144 ); 145 /* X-Dash R */ DashingInterval = (int) (XlengthRight / XdashesRight); 146 if (!(DashingInterval<2)) 147 for (int i=OriginX; i<=XmaxPx; i+=DashingInterval) 148 CommandToTFT ( "PL(" + String(i) + "," + String(OriginY-2) + "," + 149 String(i) + "," + String(OriginY+2) + "," + 150 String(ColorX) + ");" 151 ); 152 /* Y-Dash-L */ DashingInterval = (int) (YlengthLower / YdashesLower); 153 if (!(DashingInterval<2)) 154 for (int i=OriginY; i<=YminPx; i+=DashingInterval) 155 CommandToTFT ( "PL(" + String(OriginX-2) + "," + String(i) + "," + 156 String(OriginX+2) + "," + String(i) + "," + 157 String(ColorY) + ");" 158 ); 159 /* Y-Dash-U */ DashingInterval = (int) (YlengthUpper / YdashesUpper); 160 if (!(DashingInterval<2)) 161 for (int i=OriginY; i>=YmaxPx; i-=DashingInterval) 162 CommandToTFT ( "PL(" + String(OriginX-2) + "," + String(i) + "," + 163 String(OriginX+2) + "," + String(i) + "," + 164 String(ColorY) + ");" 165 ); 166 167 /* Calculating coordinates to display axis endpoint values */ 168 int XminTxtX = Window_X1 - (int)(XminTxt.length()*6) - 1, 169 XminTxtY = OriginY, 170 XmaxTxtX = Window_X2 + 1, 171 XmaxTxtY = OriginY, 172 YminTxtX = OriginX, 173 YminTxtY = Window_Y2 + 1, 174 YmaxTxtX = OriginX, 175 YmaxTxtY = Window_Y1 - 12 - 1; 176 /* Controls: If any coordinate is -1, it shall fall beyond display limits 177 and respective value shall not be displayed */ 178 if (XminTxtX<0) XminTxtX = -1; 179 if ( (XminTxtY-12) < 0 ) XminTxtY = -1; 180 if ( (XmaxTxtX+6*XmaxTxt.length()) > DisplayResolutionX ) XmaxTxtX = -1; 181 if ( (XmaxTxtY+12) > DisplayResolutionY ) XmaxTxtY = -1; 182 if ( (YminTxtX+6*YminTxt.length()) > DisplayResolutionX ) YminTxtX = -1; 183 if ( (YminTxtY+12) > DisplayResolutionY ) YminTxtY = -1; 184 if ( (YmaxTxtX+6*YmaxTxt.length()) > DisplayResolutionX ) YmaxTxtX = -1; 185 if (YmaxTxtY<0) YmaxTxtY = -1; 186 187 /* Range Limit Titles */ 188 if ( ( XminTxtX != -1 ) && ( XminTxtY != -1 ) ) 189 CommandToTFT( "DS12(" + String(XminTxtX) + "," + String(XminTxtY) + ",'" + String(XminTxt) + "'," + String(ColorX) + ");" ); 190 if ( ( XmaxTxtX != -1 ) && ( XmaxTxtY != -1 ) ) 191 CommandToTFT( "DS12(" + String(XmaxTxtX) + "," + String(XmaxTxtY) + ",'" + String(XmaxTxt) + "'," + String(ColorX) + ");" ); 192 if ( ( YminTxtX != -1 ) && ( YminTxtY != -1 ) ) 193 CommandToTFT( "DS12(" + String(YminTxtX) + "," + String(YminTxtY) + ",'" + String(YminTxt) + "'," + String(ColorY) + ");" ); 194 if ( ( YmaxTxtX != -1 ) && ( YmaxTxtY != -1 ) ) 195 CommandToTFT( "DS12(" + String(YmaxTxtX) + "," + String(YmaxTxtY) + ",'" + String(YmaxTxt) + "'," + String(ColorY) + ");" ); 196 197 /* 198 Return Value String 199 Cartesian_Setup() will return a string packing graphic configuration in following format: 200 "<Xmin,Xmax,Ymin,Ymax,Window_X1,Window_Y1,Window_X2,Window_Y2>" 201 String starts with '<' and ends by '>'. Each value is delimited by ',' 202 */ 203 /* Initialize */ String Cartesian_SetupDetails = "<"; 204 Cartesian_SetupDetails += ( String(Xmin) + "," ); 205 Cartesian_SetupDetails += ( String(Xmax) + "," ); 206 Cartesian_SetupDetails += ( String(Ymin) + "," ); 207 Cartesian_SetupDetails += ( String(Ymax) + "," ); 208 Cartesian_SetupDetails += ( String(Window_X1) + "," ); 209 Cartesian_SetupDetails += ( String(Window_Y1) + "," ); 210 Cartesian_SetupDetails += ( String(Window_X2) + "," ); 211 Cartesian_SetupDetails += ( String(Window_Y2) + "," ); 212 /* Close-Out */ Cartesian_SetupDetails += ">"; 213 214return Cartesian_SetupDetails; 215} 216/* ########### End of Cartesian_Setup() ########### */ 217/* ################################################ */ 218 219/* ############################################### 220 Cartesian_ClearPlotAreas(Descriptor, Color) 221 Plot Area Reset/Clear Function for Makeblock Me TFT LCD 222 Input Parameters: 223 (String) Descriptor : Setup Descriptor - returned by Cartesian_Setup() 224 (int) Color______: Color to be used to fill plot area 225 Uses external function CommandToTFT(). 226############################################### */ 227void Cartesian_ClearPlotAreas(String Descriptor, int Color) 228{ 229 int X1,Y1,X2,Y2; /* Boundary coordinates for plot areas */ 230 /* Extracting values from Descriptor */ 231 /* L[0] L[1] L[2] L[3] W[0] W[1] W[2] W[3] */ 232 /* Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2 */ 233 float L[4]; int W[4]; /* Values stored in Descriptor */ 234 int j=0; /* Counter */ 235 String D_Str = ""; 236 for (int i=1; i<=(Descriptor.length()-1); i++) 237 if ( Descriptor[i] == ',' ) 238 { 239 if (j<4) L[j]=D_Str.toFloat(); else W[j-4]=D_Str.toInt(); 240 D_Str=""; j++; 241 } 242 else 243 D_Str += Descriptor[i]; 244 245 /* Origin */ int OriginX = (W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) / (abs(L[1])+abs(L[0])) ); 246 int OriginY = (W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3])+abs(L[2])) ); 247 248 /* Clearing Plot Areas */ 249 //Area.1 : X+ Y+ 250 X1 = OriginX + 2 ; Y1 = W[1] + 1 ; 251 X2 = W[2] - 1 ; Y2 = OriginY - 2 ; 252 CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 253 String(X2) + "," + String(Y2) + "," + 254 String(Color) + ");" 255 ); 256 //Area.2 : X- Y+ 257 X1 = W[0] + 1 ; Y1 = W[1] + 1 ; 258 X2 = OriginX - 2 ; Y2 = OriginY - 2 ; 259 CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 260 String(X2) + "," + String(Y2) + "," + 261 String(Color) + ");" 262 ); 263 //Area.3 : X- Y- 264 X1 = W[0] + 1 ; Y1 = OriginY + 2 ; 265 X2 = OriginX - 2 ; Y2 = W[3] - 1 ; 266 CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 267 String(X2) + "," + String(Y2) + "," + 268 String(Color) + ");" 269 ); 270 //Area.4 : X+ Y- 271 X1 = OriginX + 2 ; Y1 = OriginY + 2 ; 272 X2 = W[2] - 1 ; Y2 = W[3] - 1 ; 273 CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 274 String(X2) + "," + String(Y2) + "," + 275 String(Color) + ");" 276 ); 277} 278/* ########### End of Cartesian_ClearPlotAreas() ########### */ 279/* ######################################################### */ 280 281/* ############################################### 282 Cartesian_Line(Xp, Yp, X, Y, Descriptor, Color) 283 Cartesian Line Function for Makeblock Me TFT LCD 284 Input Parameters: 285 (int) Xp, Yp_____: Previous plot coordinates - y value vs x 286 (int) X, Y_______: Current plot coordinates - y value vs x 287 (String) Descriptor : Setup Descriptor - returned by Cartesian_Setup() 288 (int) Color______: Marking color to be used on (x,y) 289 Uses external function CommandToTFT(). 290############################################### */ 291void Cartesian_Line(float Xp, float Yp, float X, float Y, String Descriptor, int Color) 292{ 293 /* Extracting values from Descriptor */ 294 /* L[0] L[1] L[2] L[3] W[0] W[1] W[2] W[3] */ 295 /* Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2 */ 296 float L[4]; int W[4]; /* Values stored in Descriptor */ 297 int j=0; /* Counter */ 298 String D_Str = ""; 299 for (int i=1; i<=(Descriptor.length()-1); i++) 300 if ( Descriptor[i] == ',' ) 301 { 302 if (j<4) L[j]=D_Str.toFloat(); else W[j-4]=D_Str.toInt(); 303 D_Str=""; j++; 304 } 305 else 306 D_Str += Descriptor[i]; 307 308 /* Origin */ int OriginX = (W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) / (abs(L[1])+abs(L[0])) ); 309 int OriginY = (W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3])+abs(L[2])) ); 310 311 int XminPx = W[0] + 1; 312 int XmaxPx = W[2] - 1; 313 int YmaxPx = W[1] + 1; 314 int YminPx = W[3] - 1; 315 316 if (Y>L[3]) Y=L[3]; 317 if (Y<L[2]) Y=L[2]; 318 319 float RatioX = (float)( XmaxPx - XminPx ) / ( L[1] - L[0] ); 320 float RatioY = (float)abs( YmaxPx - YminPx ) / ( L[3] - L[2] ); 321 322 int DispXp = XminPx + (int)( RatioX * ( Xp - L[0] ) ); 323 int DispYp = YminPx - (int)( RatioY * ( Yp - L[2] ) ); 324 int DispX = XminPx + (int)( RatioX * ( X - L[0] ) ); 325 int DispY = YminPx - (int)( RatioY * ( Y - L[2] ) ); 326 if (!( 327 ( ( DispXp >= (OriginX-2) ) && ( DispXp <= (OriginX+2) ) ) || 328 ( ( DispYp >= (OriginY-2) ) && ( DispYp <= (OriginY+2) ) ) || 329 ( ( DispX >= (OriginX-2) ) && ( DispX <= (OriginX+2) ) ) || 330 ( ( DispY >= (OriginY-2) ) && ( DispY <= (OriginY+2) ) ) 331 )) 332 CommandToTFT( "PL(" + String(DispXp) + "," + String(DispYp) + "," + String(DispX) + "," + String(DispY) + "," + String(Color) + ");" ); 333} 334/* ########### End of Cartesian_Line() ########### */ 335/* ############################################### */ 336 337/* ############################################### 338 readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed) 339 Frequency Reading Function 340 Input Parameters: 341 (int) _DI_FrequencyCounter_Pin : Digital pin to be read 342 (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1) 343 344 Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted. 345 It cannot be 0(zero), negative values or a value greater than 10. 346 When _ReadingSpeed changed, 1 second shall be divided by this value to calculate 347 required counting duration. For example; 348 - _ReadingSpeed = 0.1 -> input shall be counted during 10 seconds (=1/0.1) 349 - _ReadingSpeed = 0.5 -> input shall be counted during 2 seconds (=1/0.5) 350 - _ReadingSpeed = 2.0 -> input shall be counted during 0.5 seconds (=1/2) 351 - _ReadingSpeed = 4.0 -> input shall be counted during 0.25 seconds (=1/4) 352 Importantly note that, increasing of _ReadingSpeed is a disadvantage especially 353 on lower frequencies (generally below 100 Hz) since counting error increases 354 up to 20%~40% by decreasing frequency. 355############################################### */ 356 357int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed) 358{ 359 pinMode(_DI_FrequencyCounter_Pin,INPUT); 360 byte _DigitalRead, _DigitalRead_Previous = 0; 361 unsigned long _Time = 0, _Time_Init; 362 float _Frequency = 0; 363 if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); 364 else 365 { 366 _Time_Init = micros(); 367 do 368 { 369 _DigitalRead = digitalRead(_DI_FrequencyCounter_Pin); 370 if ( (_DigitalRead_Previous==1) && (_DigitalRead==0) ) _Frequency++; 371 _DigitalRead_Previous = _DigitalRead; 372 _Time = micros(); 373 } 374 while ( _Time < (_Time_Init + (1000000/_ReadingSpeed)) ); 375 } 376 return (_ReadingSpeed * _Frequency); 377} 378/* ########### End of readFrequency() ########### */ 379/* ############################################## */ 380 381/* ############################################### 382 controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) 383 PID Controller Function 384 Input Parameters: 385 (float) RangeMin: Minimum limit for output 386 (float) RangeMax: Maximum limit for output 387 (float) _E_____: Current error signal 388 (float) _Eprev : Previous error signal 389 (float) _dT____: Time difference as seconds 390 (float) _Kp____: Proportional coefficient 391 (float) _Ki____: Integral coefficient 392 (float) _Kp____: Derivative coefficient 393 Adjustment procedure: 394 1. Set Kp=0, Ki=0, Kd=0. 395 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note 396 critical gain Kc = Kp. 397 3. Adjust final coefficients as follows. 398 for P-control only : Kp = 0.50*Kc 399 for PI-control only : Kp = 0.45*Kc, Ki = 1.2/Pc 400 for PID-control : Kp = 0.60*Kc, Ki = 2.0/Pc, Kd=Pc/8 401 4. Fine tuning could be done by slightly changing each coefficient. 402############################################### */ 403 404float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd) 405{ 406 float P, I, D; 407 /* 408 Base Formula: U = _Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); 409 */ 410 P = _Kp * _E; /* Proportional Component */ 411 I = _Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ 412 D = _Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ 413 return (P+I+D); 414} 415/* ########### End of controllerPID() ########### */ 416/* ############################################## */ 417 418/* ############################################### 419 Setup 420############################################### */ 421 422void setup() 423{ 424 Serial1.begin(9600); 425 Serial1.println("CLS(0);");delay(20); 426 analogReadResolution(12); 427 428 pinMode(_chDirection,INPUT); // Direction selector reading 429 pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn 430 pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn 431 432 // Initial killing the PWM outputs to motor 433 analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); 434 435 // Initial reading for direction selection 436 Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW 437 prevDirection=Direction; 438 439 // The section below prepares TFT LCD 440 // Cartesian_Setup(Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX, ColorY) 441 Cartesian_SetupDetails = Cartesian_Setup(0, _Tmax, _minRPM, _maxRPM, 20, 20, 220, 120, 10, 0, 7, 7); 442 CommandToTFT("DS12(250,10,'Dir: CW '," + String(_WHITE) + ");"); 443 CommandToTFT("DS12(250,25,'____ Set'," + String(_YELLOW) + ");"); 444 CommandToTFT("DS12(250,40,'____ RPM'," + String(_GREEN) + ");"); 445 /* Alarm Values */ 446 CommandToTFT("DS12(250,55,'AHH:" + String(RAHH) + "'," + String(_WHITE) + ");"); 447 CommandToTFT("DS12(250,70,'AH :" + String(RAH) + "'," + String(_WHITE) + ");"); 448 CommandToTFT("DS12(250,85,'AL :" + String(RAL) + "'," + String(_WHITE) + ");"); 449 CommandToTFT("DS12(250,100,'ALL:"+ String(RALL) + "'," + String(_WHITE) + ");"); 450 /* Alarm Window */ 451 CommandToTFT("BOX(240,55,319,115," + String(_WHITE) + ");"); 452 /* Alarm Lamps */ 453 CommandToTFT("BOX(240,55,248,70," + String(_WHITE) + ");"); 454 CommandToTFT("BOX(240,70,248,85," + String(_WHITE) + ");"); 455 CommandToTFT("BOX(240,85,248,100," + String(_WHITE) + ");"); 456 CommandToTFT("BOX(240,100,248,115," + String(_WHITE) + ");"); 457} 458 459 460/* ############################################### 461 Loop 462############################################### */ 463 464void loop() 465{ 466 467 // Initialization Time: Necessary for PID controller. 468 int InitTime = micros(); 469 470 // X-Axis Auto-Reset for Graphing 471 if ( Seconds > 90.0 ) 472 { 473 Seconds = 0.0; 474 Cartesian_ClearPlotAreas(Cartesian_SetupDetails,0); 475 } 476 477 // Reading Inputs 478 /* Controller Coefficients */ 479 Kp = Kpmax * (float)analogRead(_chKp) / 4095; 480 Ki = Kimax * (float)analogRead(_chKi) / 4095; 481 Kd = Kdmax * (float)analogRead(_chKd) / 4095; 482 /* Direction Selector */ 483 Direction = digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ 484 /* Actual RPM and RPM Setpoint 485 Note that maximum selectable RPM is 5000. */ 486 RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; 487 RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095; 488 489 490 // Calculations and Actions 491 /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ 492 E = RPMset - RPM; 493 float cPID = controllerPID(E, Eprev, dT, Kp, Ki, Kd); 494 if ( RPMset == 0 ) OutputRPM = 0; 495 else OutputRPM = OutputRPM + cPID; 496 if ( OutputRPM < _minRPM ) OutputRPM = _minRPM; 497 if ( OutputRPM > _maxRPM ) OutputRPM = _maxRPM; 498 499 /* Changing Direction when inverted 500 Note that no any graphical indication is performed on this function. */ 501 if ( Direction != prevDirection ) 502 { 503 /* Killing both of the PWM outputs to motor */ 504 analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); 505 /* Wait until motor speed decreases */ 506 do 507 { RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } 508 while ( RPM > _minRPM ); 509 } 510 511 // Writing Outputs 512 if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); 513 else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); 514 515 // Graphing 516 /* Indicating Direction */ 517 if (Direction==HIGH) CommandToTFT("DS12(280,10,'CCW '," + String(_WHITE) + ");"); 518 else CommandToTFT("DS12(280,10,'CW '," + String(_WHITE) + ");"); 519 /* Plotting Curve */ 520 Cartesian_Line(prevSeconds, prevRPMset, Seconds, RPMset, Cartesian_SetupDetails, _YELLOW); 521 Cartesian_Line(prevSeconds, prevRPM, Seconds, RPM, Cartesian_SetupDetails, _GREEN); 522 /* Indicating values of RPM Setpoint, PID Controller Coefficients, 523 Error Signal, PID Controller Output and Final RPM Output (PWM) */ 524 CommandToTFT( "DS12(20,150,'Set: " + String(RPMset) + " rpm " + 525 "RPM: " + String(RPM) + " rpm '," + String(_WHITE) + ");"); 526 CommandToTFT( "DS12(20,170,'Kp= " + String(Kp) + " " + 527 "Ki= " + String(Ki) + " " + 528 "Kd= " + String(Kd) + " " + 529 "dT= " + String(dT*1000) + " ms '," + String(_WHITE) + ");"); 530 CommandToTFT( "DS12(20,190,'e= " + String(E) + " " + 531 "cPID= " + String(cPID) + " " + 532 "RPMout= " + String(OutputRPM) + " '," + String(_WHITE) + ");"); 533 /* Resetting Alarm Lamps */ 534 CommandToTFT("BOXF(241,56,247,69," + String(_BLACK) + ");"); 535 CommandToTFT("BOXF(241,71,247,84," + String(_BLACK) + ");"); 536 CommandToTFT("BOXF(241,86,247,99," + String(_BLACK) + ");"); 537 CommandToTFT("BOXF(241,101,247,114," + String(_BLACK) + ");"); 538 /* Activating Necessary Alarm Lamps */ 539 if (RPM>=RAHH) CommandToTFT("BOXF(241,56,247,69," + String(_RED) + ");"); 540 if ((RPM>=RAH)&&(RPM<RAHH)) CommandToTFT("BOXF(241,71,247,84," + String(_RED) + ");"); 541 if ((RPM>RALL)&&(RPM<=RAL)) CommandToTFT("BOXF(241,86,247,99," + String(_RED) + ");"); 542 if (RPM<=RALL) CommandToTFT("BOXF(241,101,247,114," + String(_RED) + ");"); 543 544 // Storing Values generated on previous cycle 545 Eprev = E; prevRPMset = RPMset; prevRPM = RPM; 546 prevSeconds = Seconds; prevDirection = Direction; 547 548 // Calculating control application cycle time and passed Seconds 549 dT = float ( micros() - InitTime ) / 1000000.0; 550 Seconds+=dT; 551}
Code without Graphical Touch-Ups
arduino
1/* ############################################### 2 I/O Assignments 3############################################### */ 4int _chSpeedSet = A0, // Speed setpoint 5 _chKp = A1, // Proportional coefficient reading for PID controller 6 _chKi = A2, // Integral coefficient reading for PID controller 7 _chKd = A3, // Derivative coefficient reading for PID controller 8 _chMotorCmdCCW = 3, // PWM output to motor for counter-clockwise turn 9 _chMotorCmdCW = 2, // PWM output to motor for clockwise turn 10 _chSpeedRead = 24, // Speed reading 11 _chDirection = 25; // Direction selector reading 12 13/* ############################################### 14 Other Constants 15############################################### */ 16#define _minRPM 0 // Minimum RPM to initiate direction changing 17#define _maxRPM 6000 // Maximum RPM limit 18#define _DiscSlots 20 // Qty of slots on Index Disc 19 20/* ############################################### 21 Global Variables 22############################################### */ 23boolean Direction, prevDirection; 24float RPM=0.0, RPMset=0.0, OutputRPM=0.0, 25 Kp=0.0, Ki=0.0, Kd=0.0, 26 Kpmax=2.0, Kimax=1.0, Kdmax=1.0, 27 E=0.0, Eprev=0.0, dT=1.0; 28 29/* ############################################### 30 readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed) 31 Frequency Reading Function 32 Input Parameters: 33 (int) _DI_FrequencyCounter_Pin : Digital pin to be read 34 (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1) 35 36 Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted. 37 It cannot be 0(zero), negative values or a value greater than 10. 38 When _ReadingSpeed changed, 1 second shall be divided by this value to calculate 39 required counting duration. For example; 40 - _ReadingSpeed = 0.1 -> input shall be counted during 10 seconds (=1/0.1) 41 - _ReadingSpeed = 0.5 -> input shall be counted during 2 seconds (=1/0.5) 42 - _ReadingSpeed = 2.0 -> input shall be counted during 0.5 seconds (=1/2) 43 - _ReadingSpeed = 4.0 -> input shall be counted during 0.25 seconds (=1/4) 44 Importantly note that, increasing of _ReadingSpeed is a disadvantage especially 45 on lower frequencies (generally below 100 Hz) since counting error increases 46 up to 20%~40% by decreasing frequency. 47############################################### */ 48 49int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed) 50{ 51 pinMode(_DI_FrequencyCounter_Pin,INPUT); 52 byte _DigitalRead, _DigitalRead_Previous = 0; 53 unsigned long _Time = 0, _Time_Init; 54 float _Frequency = 0; 55 if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); 56 else 57 { 58 _Time_Init = micros(); 59 do 60 { 61 _DigitalRead = digitalRead(_DI_FrequencyCounter_Pin); 62 if ( (_DigitalRead_Previous==1) && (_DigitalRead==0) ) _Frequency++; 63 _DigitalRead_Previous = _DigitalRead; 64 _Time = micros(); 65 } 66 while ( _Time < (_Time_Init + (1000000/_ReadingSpeed)) ); 67 } 68 return (_ReadingSpeed * _Frequency); 69} 70/* ########### End of readFrequency() ########### */ 71/* ############################################## */ 72 73/* ############################################### 74 controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) 75 PID Controller Function 76 Input Parameters: 77 (float) RangeMin: Minimum limit for output 78 (float) RangeMax: Maximum limit for output 79 (float) _E_____: Current error signal 80 (float) _Eprev : Previous error signal 81 (float) _dT____: Time difference as seconds 82 (float) _Kp____: Proportional coefficient 83 (float) _Ki____: Integral coefficient 84 (float) _Kp____: Derivative coefficient 85 Adjustment procedure: 86 1. Set Kp=0, Ki=0, Kd=0. 87 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note 88 critical gain Kc = Kp. 89 3. Adjust final coefficients as follows. 90 for P-control only : Kp = 0.50*Kc 91 for PI-control only : Kp = 0.45*Kc, Ki = 1.2/Pc 92 for PID-control : Kp = 0.60*Kc, Ki = 2.0/Pc, Kd=Pc/8 93 4. Fine tuning could be done by slightly changing each coefficient. 94############################################### */ 95 96float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd) 97{ 98 float P, I, D; 99 /* 100 Base Formula: U = _Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); 101 */ 102 P = _Kp * _E; /* Proportional Component */ 103 I = _Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ 104 D = _Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ 105 return (P+I+D); 106} 107/* ########### End of controllerPID() ########### */ 108/* ############################################## */ 109 110/* ############################################### 111 Setup 112############################################### */ 113 114void setup() 115{ 116 analogReadResolution(12); 117 pinMode(_chDirection,INPUT); // Direction selector reading 118 pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn 119 pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn 120 // Initial killing the PWM outputs to motor 121 analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); 122 // Initial reading for direction selection 123 Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW 124 prevDirection=Direction; 125} 126 127 128/* ############################################### 129 Loop 130############################################### */ 131 132void loop() 133{ 134 // Initialization Time: Necessary for PID controller. 135 int InitTime = micros(); 136 137 // Reading Inputs 138 /* Controller Coefficients */ 139 Kp = Kpmax * (float)analogRead(_chKp) / 4095; 140 Ki = Kimax * (float)analogRead(_chKi) / 4095; 141 Kd = Kdmax * (float)analogRead(_chKd) / 4095; 142 /* Direction Selector */ 143 Direction = digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ 144 /* Actual RPM and RPM Setpoint 145 Note that maximum selectable RPM is 5000. */ 146 RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; 147 RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095; 148 149 // Calculations and Actions 150 /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ 151 E = RPMset - RPM; 152 float cPID = controllerPID(E, Eprev, dT, Kp, Ki, Kd); 153 if ( RPMset == 0 ) OutputRPM = 0; 154 else OutputRPM = OutputRPM + cPID; 155 if ( OutputRPM < _minRPM ) OutputRPM = _minRPM; 156 if ( OutputRPM > _maxRPM ) OutputRPM = _maxRPM; 157 158 /* Changing Direction when inverted */ 159 if ( Direction != prevDirection ) 160 { 161 /* Killing both of the PWM outputs to motor */ 162 analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); 163 /* Wait until motor speed decreases */ 164 do 165 { RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } 166 while ( RPM > _minRPM ); 167 } 168 169 // Writing Outputs 170 if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); 171 else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); 172 173 // Storing Values generated on previous cycle 174 Eprev = E; prevDirection = Direction; 175 176 // Calculating control application cycle time and passed Seconds 177 dT = float ( micros() - InitTime ) / 1000000.0; 178}
Downloadable files
Schematic for Control of Direction and Speed of a DC Motor
It's a prototype to explain DC motor speed control by using PID controller, and what should be considered for reversing.
Schematic for Control of Direction and Speed of a DC Motor
Schematic for Control of Direction and Speed of a DC Motor
It's a prototype to explain DC motor speed control by using PID controller, and what should be considered for reversing.
Schematic for Control of Direction and Speed of a DC Motor
Comments
Only logged in users can leave comments
tolgadurudogan
0 Followers
•0 Projects
Table of contents
Intro
65
0