Motor Controller for the SkyFawkes robot entering PiWars 2019

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }