Motor Controller for the SkyFawkes robot entering PiWars 2019
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 00003 //Serial pc(SERIAL_TX, SERIAL_RX); 00004 00005 00006 //Analog inputs A0-A5 addressable as such. A6 to A9 do not work. 00007 AnalogIn ANALOG0(A0); 00008 AnalogIn ANALOG1(A1); 00009 AnalogIn ANALOG2(A2); 00010 AnalogIn ANALOG3(A3); 00011 AnalogIn ANALOG4(A4); 00012 AnalogIn ANALOG5(A5); 00013 SPISlave rpi(PB_5, PB_4, PB_3, PA_15); //setup SPI pins to talk to the raspberry pi 00014 //PA_9 - MLB //PA_8 - MLF //PA_10 - MRF //PA_11 - MRB 00015 PwmOut MRB(PA_11); //back right motor 00016 PwmOut MRF(PA_10); //front right motor 00017 PwmOut MLB(PA_9); //back left motor 00018 PwmOut MLF(PA_8); //front left motor 00019 DigitalOut RAIN1(PC_6); //PC_9 - Left //PC_6 - Right 00020 DigitalOut RAIN2(PB_9); // PC_8 - Left //PB_9 - Right 00021 DigitalOut RSTANDBY(PC_5); //PB_8 - Left //PC_5 - Right 00022 DigitalOut LAIN1(PC_9); 00023 DigitalOut LAIN2(PC_8); 00024 DigitalOut LSTANDBY(PB_8); 00025 //InterruptIn MRB_ENC1(PB_6); 00026 //InterruptIn MRB_ENC2(PC_7); 00027 InterruptIn MRF_ENC1(PA_13); 00028 InterruptIn MRF_ENC2(PB_7); 00029 //InterruptIn MLB_ENC1(PD_2); 00030 //InterruptIn MLB_ENC2(PC_12); 00031 InterruptIn MLF_ENC1(PC_11); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB 00032 InterruptIn MLF_ENC2(PC_10); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB 00033 Ticker control_timer; 00034 00035 bool control_loop_flag = false; 00036 00037 //motor associations within arrays 00038 //Right = 0; Left = 1 00039 int m_count [2] = {0,0}; //setup array for 2 encoder counts 00040 int m_last_count [2] = {0,0}; //setup array for storing previous encoder counts 00041 int m_speed_ref [2] = {0,0}; //setup array for 2 motor speeds 00042 int m_top_speed [2] = {0,0}; 00043 float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty 00044 int m_distance_ref [2] = {0,0}; //setup array for 2 motor distances 00045 int max_accel = 5; //for storing the maximum acceleration 00046 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values 00047 int distance_scale = 14; 00048 //max number over SPI is 100. Scale to make this equivalent to 1m 00049 //with 48mm diameter wheels and 210 counts per revolution. Circumference = pi*D = 0.1508m per rev. 00050 //1m/0.1508m = 6.63. 6.63*210 = 1392 counts. So scale to make 100 = 1400 counts 00051 00052 float integral [2] = {0,0}; 00053 bool control_mode = true; //control mode flag. True is speed control and False is distance control 00054 //bool m_direction [2] = {true, true}; 00055 00056 void readADC(); //read 6 channels of ADC 00057 void setDuty(); //set the 4 motor PWM duty cycles 00058 void setPeriod(int period); //set PWM period for motors in microseconds 00059 void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back. 00060 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back. 00061 void spiComms(); //do spi communications with raspberry pi 00062 00063 void mrfEncoderIsr1(); 00064 void mrfEncoderIsr2(); 00065 void mlfEncoderIsr1(); 00066 void mlfEncoderIsr2(); 00067 void controlTick(); 00068 float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed) ; 00069 00070 00071 int main() 00072 { 00073 float pos_change = 0; //temp variable for speed control 00074 float speed_error = 0; //temp variable for speed control 00075 00076 float Kp = 0.02; //proportional constant 00077 float Ki = 0.05; //integral constant 00078 int i = 0; 00079 00080 // float distance_error = 0; 00081 // float Kp_d = 0.01; 00082 // float max_duty = 0; 00083 00084 00085 //setup interrupts for encoders 00086 MRF_ENC1.fall(&mrfEncoderIsr1); 00087 MRF_ENC2.fall(&mrfEncoderIsr2); 00088 MLF_ENC1.fall(&mlfEncoderIsr1); 00089 MLF_ENC2.fall(&mlfEncoderIsr2); 00090 00091 //setup driver pins 00092 setLMotorDir(1); 00093 setRMotorDir(1); 00094 // Set PWM period in us 00095 setPeriod(100); 00096 // pc.printf("Starting up"); 00097 //setup control loop 00098 control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms 00099 00100 00101 00102 while (1) { 00103 // pc.printf("Motor count %i\r\n", m_count[0]); 00104 00105 if(control_loop_flag == true) { //flag set true by ticker timer every 50ms 00106 if(control_mode == true) { //speed control 00107 for(i=0; i<=1; i++) { //do this for right and left in turn 00108 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position 00109 m_last_count[i] = m_count[i]; //store count for next cycle 00110 speed_error = pos_change - float(m_speed_ref[i]); //calculate different between current speed and reference speed 00111 integral[i] = integral[i] + speed_error; 00112 m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control 00113 } 00114 if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill 00115 m_duty[0] = 0; 00116 if(m_speed_ref[1] == 0) 00117 m_duty[1] = 0; 00118 } else if(control_mode == false) { //distance x and max speed y control 00119 int targetSpeed=0; 00120 for(i=0; i<=1; i++) { //do this for right and left in turn 00121 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position 00122 m_last_count[i] = m_count[i]; //store count for next cycle 00123 targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i]);//m_speed_ref is used as top speed 00124 speed_error = pos_change - targetSpeed; //calculate different between current speed and reference speed 00125 integral[i] = integral[i] + speed_error; 00126 m_duty[i] = Kp*0.1*speed_error + Ki*0.1*integral[i]; //speed proportional control 00127 } 00128 } 00129 00130 if(m_duty[0] < 0) { 00131 setRMotorDir(0); //set the motors backwards 00132 m_duty[0] = -1*m_duty[0]; //make the negative value into positive 00133 } else { 00134 setRMotorDir(1); //set the motors forwards 00135 } 00136 if(m_duty[0] > 100) { 00137 m_duty[0] = 100; //make sure the speed isn't greater than 100% 00138 } 00139 00140 if(m_duty[1] < 0) { 00141 setLMotorDir(0); //set the motors backwards 00142 m_duty[1] = -1*m_duty[1]; //make the negative value into positive 00143 } else { 00144 setLMotorDir(1); //set the motors forwards 00145 } 00146 if(m_duty[1] > 100) { 00147 m_duty[1] = 100; //make sure the speed isn't greater than 100% 00148 } 00149 setDuty(); //set all the duty cycles to the motor drivers 00150 control_loop_flag = false; 00151 } 00152 readADC(); //read all the ADC values when not doing something else 00153 spiComms(); //do SPI communication stuff 00154 } 00155 } 00156 00157 00158 //function to read all 6 ADC channels 00159 void readADC() 00160 { 00161 adc_values[0] = int(ANALOG0.read()*255); 00162 adc_values[1] = int(ANALOG1.read()*255); 00163 adc_values[2] = int(ANALOG2.read()*255); 00164 adc_values[3] = int(ANALOG3.read()*255); 00165 adc_values[4] = int(ANALOG4.read()*255); 00166 adc_values[5] = int(ANALOG5.read()*255); 00167 } 00168 00169 //function to set all 4 motor PWM duty cycles 00170 void setDuty() 00171 { 00172 MRB.write(m_duty[0]); 00173 MRF.write(m_duty[0]); 00174 MLB.write(m_duty[1]); 00175 MLF.write(m_duty[1]); 00176 } 00177 //set left motor direction. 1 is forward, 0 is backwards. 00178 void setLMotorDir(bool direction) 00179 { 00180 LSTANDBY = 1; 00181 if(direction == true) { 00182 LAIN1 = 1; 00183 LAIN2 = 0; 00184 } else if(direction == false) { 00185 LAIN1 = 0; 00186 LAIN2 = 1; 00187 } 00188 } 00189 //set right motor direction. 1 is forward, 0 is backwards. 00190 void setRMotorDir(bool direction) 00191 { 00192 RSTANDBY = 1; 00193 if(direction == true) { 00194 RAIN1 = 0; 00195 RAIN2 = 1; 00196 } else if(direction == false) { 00197 RAIN1 = 1; 00198 RAIN2 = 0; 00199 } 00200 } 00201 00202 //initialisation function to set motor PWM period and set to 0 duty 00203 void setPeriod(int period) 00204 { 00205 MRB.period_us(period); 00206 MRB.write(0.0); 00207 MRF.period_us(period); 00208 MRF.write(0.0); 00209 MLB.period_us(period); 00210 MLB.write(0.0); 00211 MLF.period_us(period); 00212 MLF.write(0.0); 00213 } 00214 00215 void spiComms() 00216 { 00217 //do SPI communication stuff 00218 int i = 0; //temp counter variable 00219 int v = 0; //temp SPI variable 00220 if(rpi.receive()) { 00221 v = rpi.read(); // Read byte from master 00222 if(v == char('S')) { 00223 rpi.reply(0x01); 00224 for (i=0; i<=1; i++) { 00225 m_speed_ref[i] = rpi.read() - 128; 00226 rpi.reply(m_speed_ref[i]); 00227 } 00228 v = rpi.read(); //last bit setup a blank reply 00229 rpi.reply(0x00); 00230 control_mode = true; //set the controller go do distance control 00231 m_count[0] = 0; //reset encoder counts to avoid overflow 00232 m_count[1] = 0; 00233 integral[0] = 0; //reset integrals to avoid odd behaviour 00234 integral[1] = 0; 00235 } else if(v == char('D')) { 00236 rpi.reply(0x02); 00237 for (i=0; i<=1; i++) { 00238 m_distance_ref[i] = rpi.read() - 128; 00239 rpi.reply(m_distance_ref[i]); 00240 m_distance_ref[i] = m_distance_ref[i] * distance_scale; 00241 } 00242 for (i=0; i<=1; i++) { 00243 m_top_speed[i] = rpi.read() - 128; 00244 rpi.reply(m_top_speed[i]); 00245 } 00246 v = rpi.read(); //last bit setup a blank reply 00247 rpi.reply(0x00); 00248 control_mode = false; //set the controller go do distance control 00249 m_count[0] = 0; //reset encoder counts to avoid overflow 00250 m_count[1] = 0; 00251 m_last_count[0] = 0; 00252 m_last_count[1] = 0; 00253 } else if(v == char('A')) { 00254 rpi.reply(0x03); 00255 max_accel = rpi.read(); 00256 rpi.reply(max_accel); 00257 v = rpi.read(); //last bit setup a blank reply 00258 rpi.reply(0x00); 00259 } else if(v == char('V')) { 00260 rpi.reply(0x04); 00261 v = rpi.read(); 00262 if(v <= 6) { //check the ADC to be addressed exists 00263 rpi.reply(adc_values[v]); 00264 } 00265 v = rpi.read(); //last bit setup a blank reply 00266 rpi.reply(0x00); 00267 } 00268 } 00269 } 00270 void mrfEncoderIsr1() 00271 { 00272 if(MRF_ENC2 == 0) { 00273 m_count[0] ++; 00274 } else { 00275 m_count[0] --; 00276 } 00277 } 00278 void mrfEncoderIsr2() 00279 { 00280 if(MRF_ENC1 == 1) { 00281 m_count[0] ++; 00282 } else { 00283 m_count[0] --; 00284 } 00285 } 00286 void mlfEncoderIsr1() 00287 { 00288 if(MLF_ENC2 == 0) { 00289 m_count[1] ++; 00290 } else { 00291 m_count[1] --; 00292 } 00293 } 00294 void mlfEncoderIsr2() 00295 { 00296 if(MLF_ENC1 == 1) { 00297 m_count[1] ++; 00298 } else { 00299 m_count[1] --; 00300 } 00301 } 00302 00303 void controlTick() 00304 { 00305 control_loop_flag = true; 00306 } 00307 00308 00309 float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed) 00310 { 00311 //targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], true);//m_speed_ref is used as top speed 00312 //note to self: check direction 00313 bool direction; 00314 00315 if(end<0) 00316 direction = false; 00317 else 00318 direction = true; 00319 00320 float minSpeed=1;//deg per sec - to prevent motor speed=0 and therefore jam by delay=infinity. 00321 float halfWay = 0.0; 00322 float v = 0.0; 00323 float midPointVsqrd = 0.0; 00324 halfWay=abs(start)+(abs(end)-abs(start))/2; 00325 00326 00327 if (abs(currentPosition)>halfWay) { 00328 //deaccelarate 00329 midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-abs(start)); 00330 v=sqrt(midPointVsqrd-2*accel*(abs(currentPosition)-halfWay)); 00331 } else { 00332 //accelerate 00333 //v^2=u^2+2as 00334 v=sqrt(minSpeed*minSpeed+2*accel*(abs(currentPosition)-abs(start))); 00335 } 00336 00337 00338 if (v<minSpeed) { 00339 v=minSpeed; 00340 } else if (v>topSpeed) { 00341 v = topSpeed; 00342 } 00343 00344 if (direction == false) { 00345 v = -1*v; 00346 } 00347 00348 return v; 00349 }
Generated on Wed Aug 10 2022 06:59:49 by
1.7.2