Stevie Wray / Mbed 2 deprecated SkyFawkes

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@7:948651ca4c0e, 2019-02-24 (annotated)

Committer:
Stevie
Date:
Sun Feb 24 14:25:23 2019 +0000
Revision:
7:948651ca4c0e
Parent:
6:a44d6f7626f2
Child:
8:826ec74d53c9
Ready for tuning to SkyFawkes chassis.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stevie 0:d665d69947ab 1 #include "mbed.h"
Stevie 0:d665d69947ab 2
Stevie 4:abf0070897ff 3 //Serial pc(SERIAL_TX, SERIAL_RX);
Stevie 3:497ac7d026b5 4
Stevie 3:497ac7d026b5 5
Stevie 1:08315c315df0 6 //Analog inputs A0-A5 addressable as such. A6 to A9 do not work.
Stevie 1:08315c315df0 7 AnalogIn ANALOG0(A0);
Stevie 1:08315c315df0 8 AnalogIn ANALOG1(A1);
Stevie 1:08315c315df0 9 AnalogIn ANALOG2(A2);
Stevie 1:08315c315df0 10 AnalogIn ANALOG3(A3);
Stevie 1:08315c315df0 11 AnalogIn ANALOG4(A4);
Stevie 1:08315c315df0 12 AnalogIn ANALOG5(A5);
Stevie 0:d665d69947ab 13 SPISlave rpi(PB_5, PB_4, PB_3, PA_15); //setup SPI pins to talk to the raspberry pi
Stevie 0:d665d69947ab 14 //PA_9 - MLB //PA_8 - MLF //PA_10 - MRF //PA_11 - MRB
Stevie 0:d665d69947ab 15 PwmOut MRB(PA_11); //back right motor
Stevie 0:d665d69947ab 16 PwmOut MRF(PA_10); //front right motor
Stevie 0:d665d69947ab 17 PwmOut MLB(PA_9); //back left motor
Stevie 0:d665d69947ab 18 PwmOut MLF(PA_8); //front left motor
Stevie 0:d665d69947ab 19 DigitalOut RAIN1(PC_6); //PC_9 - Left //PC_6 - Right
Stevie 0:d665d69947ab 20 DigitalOut RAIN2(PB_9); // PC_8 - Left //PB_9 - Right
Stevie 0:d665d69947ab 21 DigitalOut RSTANDBY(PC_5); //PB_8 - Left //PC_5 - Right
Stevie 0:d665d69947ab 22 DigitalOut LAIN1(PC_9);
Stevie 0:d665d69947ab 23 DigitalOut LAIN2(PC_8);
Stevie 0:d665d69947ab 24 DigitalOut LSTANDBY(PB_8);
Stevie 3:497ac7d026b5 25 //InterruptIn MRB_ENC1(PB_6);
Stevie 3:497ac7d026b5 26 //InterruptIn MRB_ENC2(PC_7);
Stevie 3:497ac7d026b5 27 InterruptIn MRF_ENC1(PA_13);
Stevie 3:497ac7d026b5 28 InterruptIn MRF_ENC2(PB_7);
Stevie 3:497ac7d026b5 29 //InterruptIn MLB_ENC1(PD_2);
Stevie 3:497ac7d026b5 30 //InterruptIn MLB_ENC2(PC_12);
Stevie 3:497ac7d026b5 31 InterruptIn MLF_ENC1(PC_11); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB
Stevie 3:497ac7d026b5 32 InterruptIn MLF_ENC2(PC_10); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB
Stevie 3:497ac7d026b5 33 Ticker control_timer;
Stevie 3:497ac7d026b5 34
Stevie 3:497ac7d026b5 35 bool control_loop_flag = false;
Stevie 3:497ac7d026b5 36
Stevie 3:497ac7d026b5 37 //motor associations within arrays
Stevie 4:abf0070897ff 38 //Right = 0; Left = 1
Stevie 5:600c5c9cbb19 39 int m_count [2] = {0,0}; //setup array for 2 encoder counts
Stevie 5:600c5c9cbb19 40 int m_last_count [2] = {0,0}; //setup array for storing previous encoder counts
Stevie 5:600c5c9cbb19 41 int m_speed_ref [2] = {0,0}; //setup array for 2 motor speeds
Stevie 6:a44d6f7626f2 42 int m_max_duty [2] = {0,0};
Stevie 4:abf0070897ff 43 float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
Stevie 5:600c5c9cbb19 44 int m_distance_ref [2] = {0,0}; //setup array for 2 motor distances
Stevie 0:d665d69947ab 45 int max_accel = 0; //for storing the maximum acceleration
Stevie 1:08315c315df0 46 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
Stevie 6:a44d6f7626f2 47 int distance_scale = 14;
Stevie 6:a44d6f7626f2 48 //max number over SPI is 100. Scale to make this equivalent to 1m
Stevie 6:a44d6f7626f2 49 //with 48mm diameter wheels and 210 counts per revolution. Circumference = pi*D = 0.1508m per rev.
Stevie 6:a44d6f7626f2 50 //1m/0.1508m = 6.63. 6.63*210 = 1392 counts. So scale to make 100 = 1400 counts
Stevie 6:a44d6f7626f2 51
Stevie 7:948651ca4c0e 52 float integral [2] = {0,0};
Stevie 6:a44d6f7626f2 53 bool control_mode = true; //control mode flag. True is speed control and False is distance control
Stevie 0:d665d69947ab 54
Stevie 2:00535b62c344 55 void readADC(); //read 6 channels of ADC
Stevie 2:00535b62c344 56 void setDuty(); //set the 4 motor PWM duty cycles
Stevie 2:00535b62c344 57 void setPeriod(int period); //set PWM period for motors in microseconds
Stevie 2:00535b62c344 58 void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 59 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 60 void spiComms(); //do spi communications with raspberry pi
Stevie 2:00535b62c344 61
Stevie 3:497ac7d026b5 62 void mrfEncoderIsr1();
Stevie 3:497ac7d026b5 63 void mrfEncoderIsr2();
Stevie 3:497ac7d026b5 64 void mlfEncoderIsr1();
Stevie 3:497ac7d026b5 65 void mlfEncoderIsr2();
Stevie 5:600c5c9cbb19 66 void controlTick();
Stevie 3:497ac7d026b5 67
Stevie 0:d665d69947ab 68
Stevie 0:d665d69947ab 69 int main()
Stevie 0:d665d69947ab 70 {
Stevie 5:600c5c9cbb19 71 float pos_change = 0; //temp variable for speed control
Stevie 5:600c5c9cbb19 72 float speed_error = 0; //temp variable for speed control
Stevie 7:948651ca4c0e 73
Stevie 5:600c5c9cbb19 74 float Kp = 0.01; //proportional constant
Stevie 5:600c5c9cbb19 75 float Ki = 0.01; //integral constant
Stevie 5:600c5c9cbb19 76 int i = 0;
Stevie 5:600c5c9cbb19 77
Stevie 6:a44d6f7626f2 78 float distance_error = 0;
Stevie 6:a44d6f7626f2 79 float Kp_d = 0.01;
Stevie 6:a44d6f7626f2 80 float max_duty = 0;
Stevie 6:a44d6f7626f2 81
Stevie 5:600c5c9cbb19 82
Stevie 3:497ac7d026b5 83 //setup interrupts for encoders
Stevie 3:497ac7d026b5 84 MRF_ENC1.fall(&mrfEncoderIsr1);
Stevie 3:497ac7d026b5 85 MRF_ENC2.fall(&mrfEncoderIsr2);
Stevie 3:497ac7d026b5 86 MLF_ENC1.fall(&mlfEncoderIsr1);
Stevie 3:497ac7d026b5 87 MLF_ENC2.fall(&mlfEncoderIsr2);
Stevie 3:497ac7d026b5 88
Stevie 0:d665d69947ab 89 //setup driver pins
Stevie 2:00535b62c344 90 setLMotorDir(1);
Stevie 2:00535b62c344 91 setRMotorDir(1);
Stevie 2:00535b62c344 92 // Set PWM period in us
Stevie 2:00535b62c344 93 setPeriod(100);
Stevie 4:abf0070897ff 94 // pc.printf("Starting up");
Stevie 5:600c5c9cbb19 95 //setup control loop
Stevie 5:600c5c9cbb19 96 control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms
Stevie 5:600c5c9cbb19 97
Stevie 5:600c5c9cbb19 98
Stevie 3:497ac7d026b5 99
Stevie 0:d665d69947ab 100 while (1) {
Stevie 4:abf0070897ff 101 // pc.printf("Motor count %i\r\n", m_count[0]);
Stevie 5:600c5c9cbb19 102
Stevie 5:600c5c9cbb19 103 if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
Stevie 6:a44d6f7626f2 104 if(control_mode == true) { //speed control
Stevie 6:a44d6f7626f2 105 for(i=0;i<=1;i++){ //do this for right and left in turn
Stevie 6:a44d6f7626f2 106 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
Stevie 6:a44d6f7626f2 107 m_last_count[i] = m_count[i]; //store count for next cycle
Stevie 6:a44d6f7626f2 108 speed_error = pos_change - m_speed_ref[i]; //calculate different between current speed and reference speed
Stevie 6:a44d6f7626f2 109 integral[i] = integral[i] + speed_error;
Stevie 6:a44d6f7626f2 110 m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
Stevie 6:a44d6f7626f2 111 }
Stevie 6:a44d6f7626f2 112 if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
Stevie 6:a44d6f7626f2 113 m_duty[0] = 0;
Stevie 6:a44d6f7626f2 114 if(m_speed_ref[1] == 0)
Stevie 6:a44d6f7626f2 115 m_duty[1] = 0;
Stevie 6:a44d6f7626f2 116 }
Stevie 6:a44d6f7626f2 117 else if(control_mode == false) { //distance x and max speed y control
Stevie 7:948651ca4c0e 118
Stevie 7:948651ca4c0e 119 for(i=0;i<=1;i++){ //do this for right and left in turn
Stevie 6:a44d6f7626f2 120 distance_error = float(m_count[i]-m_distance_ref[i]); //calculate difference between current speed and reference speed
Stevie 7:948651ca4c0e 121 m_duty[i] = Kp_d*distance_error; //speed proportional control
Stevie 6:a44d6f7626f2 122 max_duty = float(m_max_duty[i])/100;
Stevie 6:a44d6f7626f2 123 if(m_duty[i] > max_duty){ //check the max duty hasn't been exceeded
Stevie 6:a44d6f7626f2 124 m_duty[i] = max_duty;
Stevie 6:a44d6f7626f2 125 }
Stevie 6:a44d6f7626f2 126 else if(m_duty[i] < -1*max_duty){
Stevie 6:a44d6f7626f2 127 m_duty[i] = -1*max_duty;
Stevie 6:a44d6f7626f2 128 }
Stevie 7:948651ca4c0e 129 }
Stevie 5:600c5c9cbb19 130 }
Stevie 5:600c5c9cbb19 131
Stevie 6:a44d6f7626f2 132
Stevie 5:600c5c9cbb19 133
Stevie 5:600c5c9cbb19 134 if(m_duty[0] < 0){
Stevie 5:600c5c9cbb19 135 setRMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 136 m_duty[0] = -1*m_duty[0]; //make the negative value into positive
Stevie 5:600c5c9cbb19 137 } else {
Stevie 5:600c5c9cbb19 138 setRMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 139 }
Stevie 5:600c5c9cbb19 140 if(m_duty[0] > 100){
Stevie 5:600c5c9cbb19 141 m_duty[0] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 142 }
Stevie 5:600c5c9cbb19 143
Stevie 5:600c5c9cbb19 144 if(m_duty[1] < 0){
Stevie 5:600c5c9cbb19 145 setLMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 146 m_duty[1] = -1*m_duty[1]; //make the negative value into positive
Stevie 5:600c5c9cbb19 147 } else {
Stevie 5:600c5c9cbb19 148 setLMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 149 }
Stevie 5:600c5c9cbb19 150 if(m_duty[1] > 100){
Stevie 5:600c5c9cbb19 151 m_duty[1] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 152 }
Stevie 5:600c5c9cbb19 153 setDuty(); //set all the duty cycles to the motor drivers
Stevie 5:600c5c9cbb19 154 control_loop_flag = false;
Stevie 4:abf0070897ff 155 }
Stevie 4:abf0070897ff 156
Stevie 3:497ac7d026b5 157 readADC(); //read all the ADC values when not doing something else
Stevie 3:497ac7d026b5 158 spiComms(); //do SPI communication stuff
Stevie 2:00535b62c344 159 }
Stevie 2:00535b62c344 160 }
Stevie 2:00535b62c344 161
Stevie 2:00535b62c344 162 //function to read all 6 ADC channels
Stevie 2:00535b62c344 163 void readADC(){
Stevie 2:00535b62c344 164 adc_values[0] = int(ANALOG0.read()*255);
Stevie 2:00535b62c344 165 adc_values[1] = int(ANALOG1.read()*255);
Stevie 2:00535b62c344 166 adc_values[2] = int(ANALOG2.read()*255);
Stevie 2:00535b62c344 167 adc_values[3] = int(ANALOG3.read()*255);
Stevie 2:00535b62c344 168 adc_values[4] = int(ANALOG4.read()*255);
Stevie 2:00535b62c344 169 adc_values[5] = int(ANALOG5.read()*255);
Stevie 2:00535b62c344 170 }
Stevie 2:00535b62c344 171
Stevie 2:00535b62c344 172 //function to set all 4 motor PWM duty cycles
Stevie 2:00535b62c344 173 void setDuty(){
Stevie 2:00535b62c344 174 MRB.write(m_duty[0]);
Stevie 4:abf0070897ff 175 MRF.write(m_duty[0]);
Stevie 4:abf0070897ff 176 MLB.write(m_duty[1]);
Stevie 4:abf0070897ff 177 MLF.write(m_duty[1]);
Stevie 2:00535b62c344 178 }
Stevie 2:00535b62c344 179 //set left motor direction. 1 is forward, 0 is backwards.
Stevie 2:00535b62c344 180 void setLMotorDir(bool direction){
Stevie 2:00535b62c344 181 LSTANDBY = 1;
Stevie 2:00535b62c344 182 if(direction == true){
Stevie 2:00535b62c344 183 LAIN1 = 1;
Stevie 2:00535b62c344 184 LAIN2 = 0;
Stevie 2:00535b62c344 185 }
Stevie 2:00535b62c344 186 else if(direction == false){
Stevie 2:00535b62c344 187 LAIN1 = 0;
Stevie 2:00535b62c344 188 LAIN2 = 1;
Stevie 2:00535b62c344 189 }
Stevie 2:00535b62c344 190 }
Stevie 2:00535b62c344 191 //set right motor direction. 1 is forward, 0 is backwards.
Stevie 2:00535b62c344 192 void setRMotorDir(bool direction){
Stevie 2:00535b62c344 193 RSTANDBY = 1;
Stevie 2:00535b62c344 194 if(direction == true){
Stevie 2:00535b62c344 195 RAIN1 = 0;
Stevie 2:00535b62c344 196 RAIN2 = 1;
Stevie 2:00535b62c344 197 }
Stevie 2:00535b62c344 198 else if(direction == false){
Stevie 2:00535b62c344 199 RAIN1 = 1;
Stevie 2:00535b62c344 200 RAIN2 = 0;
Stevie 2:00535b62c344 201 }
Stevie 2:00535b62c344 202 }
Stevie 2:00535b62c344 203
Stevie 2:00535b62c344 204 //initialisation function to set motor PWM period and set to 0 duty
Stevie 2:00535b62c344 205 void setPeriod(int period){
Stevie 2:00535b62c344 206 MRB.period_us(period);
Stevie 2:00535b62c344 207 MRB.write(0.0);
Stevie 2:00535b62c344 208 MRF.period_us(period);
Stevie 2:00535b62c344 209 MRF.write(0.0);
Stevie 2:00535b62c344 210 MLB.period_us(period);
Stevie 2:00535b62c344 211 MLB.write(0.0);
Stevie 2:00535b62c344 212 MLF.period_us(period);
Stevie 2:00535b62c344 213 MLF.write(0.0);
Stevie 2:00535b62c344 214 }
Stevie 2:00535b62c344 215
Stevie 2:00535b62c344 216 void spiComms(){
Stevie 2:00535b62c344 217 //do SPI communication stuff
Stevie 2:00535b62c344 218 int i = 0; //temp counter variable
Stevie 2:00535b62c344 219 int v = 0; //temp SPI variable
Stevie 2:00535b62c344 220 if(rpi.receive()) {
Stevie 2:00535b62c344 221 v = rpi.read(); // Read byte from master
Stevie 2:00535b62c344 222 if(v == char('S')){
Stevie 2:00535b62c344 223 rpi.reply(0x01);
Stevie 4:abf0070897ff 224 for (i=0;i<=1;i++){
Stevie 2:00535b62c344 225 m_speed_ref[i] = rpi.read() - 128;
Stevie 2:00535b62c344 226 rpi.reply(m_speed_ref[i]);
Stevie 0:d665d69947ab 227 }
Stevie 2:00535b62c344 228 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 229 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 230 control_mode = true; //set the controller go do distance control
Stevie 6:a44d6f7626f2 231 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 232 m_count[1] = 0;
Stevie 7:948651ca4c0e 233 integral[0] = 0; //reset integrals to avoid odd behaviour
Stevie 7:948651ca4c0e 234 integral[1] = 0;
Stevie 2:00535b62c344 235 }
Stevie 2:00535b62c344 236 else if(v == char('D')){
Stevie 2:00535b62c344 237 rpi.reply(0x02);
Stevie 4:abf0070897ff 238 for (i=0;i<=1;i++){
Stevie 6:a44d6f7626f2 239 m_distance_ref[i] = rpi.read() - 128;
Stevie 6:a44d6f7626f2 240 rpi.reply(m_distance_ref[i]);
Stevie 6:a44d6f7626f2 241 m_distance_ref[i] = m_distance_ref[i] * distance_scale;
Stevie 2:00535b62c344 242 }
Stevie 4:abf0070897ff 243 for (i=0;i<=1;i++){
Stevie 6:a44d6f7626f2 244 m_max_duty[i] = rpi.read() - 128;
Stevie 6:a44d6f7626f2 245 rpi.reply(m_max_duty[i]);
Stevie 0:d665d69947ab 246 }
Stevie 2:00535b62c344 247 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 248 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 249 control_mode = false; //set the controller go do distance control
Stevie 6:a44d6f7626f2 250 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 251 m_count[1] = 0;
Stevie 2:00535b62c344 252 }
Stevie 2:00535b62c344 253 else if(v == char('A')){
Stevie 2:00535b62c344 254 rpi.reply(0x03);
Stevie 2:00535b62c344 255 max_accel = rpi.read();
Stevie 2:00535b62c344 256 rpi.reply(max_accel);
Stevie 2:00535b62c344 257 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 258 rpi.reply(0x00);
Stevie 2:00535b62c344 259 }
Stevie 2:00535b62c344 260 else if(v == char('V')){
Stevie 2:00535b62c344 261 rpi.reply(0x04);
Stevie 2:00535b62c344 262 v = rpi.read();
Stevie 2:00535b62c344 263 if(v <= 6){ //check the ADC to be addressed exists
Stevie 2:00535b62c344 264 rpi.reply(adc_values[v]);
Stevie 0:d665d69947ab 265 }
Stevie 2:00535b62c344 266 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 267 rpi.reply(0x00);
Stevie 0:d665d69947ab 268 }
Stevie 0:d665d69947ab 269 }
Stevie 3:497ac7d026b5 270 }
Stevie 4:abf0070897ff 271 void mrfEncoderIsr1(){
Stevie 4:abf0070897ff 272 if(MRF_ENC2 == 0) {
Stevie 3:497ac7d026b5 273 m_count[0] ++;
Stevie 3:497ac7d026b5 274 } else {
Stevie 3:497ac7d026b5 275 m_count[0] --;
Stevie 3:497ac7d026b5 276 }
Stevie 3:497ac7d026b5 277 }
Stevie 4:abf0070897ff 278 void mrfEncoderIsr2(){
Stevie 4:abf0070897ff 279 if(MRF_ENC1 == 1) {
Stevie 4:abf0070897ff 280 m_count[0] ++;
Stevie 3:497ac7d026b5 281 } else {
Stevie 4:abf0070897ff 282 m_count[0] --;
Stevie 3:497ac7d026b5 283 }
Stevie 3:497ac7d026b5 284 }
Stevie 4:abf0070897ff 285 void mlfEncoderIsr1(){
Stevie 4:abf0070897ff 286 if(MLF_ENC2 == 0) {
Stevie 3:497ac7d026b5 287 m_count[1] ++;
Stevie 3:497ac7d026b5 288 } else {
Stevie 3:497ac7d026b5 289 m_count[1] --;
Stevie 3:497ac7d026b5 290 }
Stevie 3:497ac7d026b5 291 }
Stevie 4:abf0070897ff 292 void mlfEncoderIsr2(){
Stevie 4:abf0070897ff 293 if(MLF_ENC1 == 1) {
Stevie 4:abf0070897ff 294 m_count[1] ++;
Stevie 3:497ac7d026b5 295 } else {
Stevie 4:abf0070897ff 296 m_count[1] --;
Stevie 3:497ac7d026b5 297 }
Stevie 3:497ac7d026b5 298 }
Stevie 5:600c5c9cbb19 299
Stevie 5:600c5c9cbb19 300 void controlTick()
Stevie 5:600c5c9cbb19 301 {
Stevie 5:600c5c9cbb19 302 control_loop_flag = true;
Stevie 5:600c5c9cbb19 303 }