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@6:a44d6f7626f2, 2019-02-24 (annotated)

Committer:
Stevie
Date:
Sun Feb 24 14:09:25 2019 +0000
Revision:
6:a44d6f7626f2
Parent:
5:600c5c9cbb19
Child:
7:948651ca4c0e
Distance x and max duty y feature now working. Tuned for 48mm wheels and 100 meaning 1m travel

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 6:a44d6f7626f2 52 bool control_mode = true; //control mode flag. True is speed control and False is distance control
Stevie 0:d665d69947ab 53
Stevie 2:00535b62c344 54 void readADC(); //read 6 channels of ADC
Stevie 2:00535b62c344 55 void setDuty(); //set the 4 motor PWM duty cycles
Stevie 2:00535b62c344 56 void setPeriod(int period); //set PWM period for motors in microseconds
Stevie 2:00535b62c344 57 void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 58 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 59 void spiComms(); //do spi communications with raspberry pi
Stevie 2:00535b62c344 60
Stevie 3:497ac7d026b5 61 void mrfEncoderIsr1();
Stevie 3:497ac7d026b5 62 void mrfEncoderIsr2();
Stevie 3:497ac7d026b5 63 void mlfEncoderIsr1();
Stevie 3:497ac7d026b5 64 void mlfEncoderIsr2();
Stevie 5:600c5c9cbb19 65 void controlTick();
Stevie 3:497ac7d026b5 66
Stevie 0:d665d69947ab 67
Stevie 0:d665d69947ab 68 int main()
Stevie 0:d665d69947ab 69 {
Stevie 5:600c5c9cbb19 70 float pos_change = 0; //temp variable for speed control
Stevie 5:600c5c9cbb19 71 float speed_error = 0; //temp variable for speed control
Stevie 5:600c5c9cbb19 72 float integral [2] = {0,0};
Stevie 5:600c5c9cbb19 73 float Kp = 0.01; //proportional constant
Stevie 5:600c5c9cbb19 74 float Ki = 0.01; //integral constant
Stevie 5:600c5c9cbb19 75 int i = 0;
Stevie 5:600c5c9cbb19 76
Stevie 6:a44d6f7626f2 77 float distance_error = 0;
Stevie 6:a44d6f7626f2 78 float Kp_d = 0.01;
Stevie 6:a44d6f7626f2 79 float Ki_d = 0.001;
Stevie 6:a44d6f7626f2 80 float integral_d [2] = {0,0};
Stevie 6:a44d6f7626f2 81 float max_duty = 0;
Stevie 6:a44d6f7626f2 82
Stevie 5:600c5c9cbb19 83
Stevie 3:497ac7d026b5 84 //setup interrupts for encoders
Stevie 3:497ac7d026b5 85 MRF_ENC1.fall(&mrfEncoderIsr1);
Stevie 3:497ac7d026b5 86 MRF_ENC2.fall(&mrfEncoderIsr2);
Stevie 3:497ac7d026b5 87 MLF_ENC1.fall(&mlfEncoderIsr1);
Stevie 3:497ac7d026b5 88 MLF_ENC2.fall(&mlfEncoderIsr2);
Stevie 3:497ac7d026b5 89
Stevie 0:d665d69947ab 90 //setup driver pins
Stevie 2:00535b62c344 91 setLMotorDir(1);
Stevie 2:00535b62c344 92 setRMotorDir(1);
Stevie 2:00535b62c344 93 // Set PWM period in us
Stevie 2:00535b62c344 94 setPeriod(100);
Stevie 4:abf0070897ff 95 // pc.printf("Starting up");
Stevie 5:600c5c9cbb19 96 //setup control loop
Stevie 5:600c5c9cbb19 97 control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms
Stevie 5:600c5c9cbb19 98
Stevie 5:600c5c9cbb19 99
Stevie 3:497ac7d026b5 100
Stevie 0:d665d69947ab 101 while (1) {
Stevie 4:abf0070897ff 102 // pc.printf("Motor count %i\r\n", m_count[0]);
Stevie 5:600c5c9cbb19 103
Stevie 5:600c5c9cbb19 104 if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
Stevie 6:a44d6f7626f2 105 if(control_mode == true) { //speed control
Stevie 6:a44d6f7626f2 106 for(i=0;i<=1;i++){ //do this for right and left in turn
Stevie 6:a44d6f7626f2 107 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
Stevie 6:a44d6f7626f2 108 m_last_count[i] = m_count[i]; //store count for next cycle
Stevie 6:a44d6f7626f2 109 speed_error = pos_change - m_speed_ref[i]; //calculate different between current speed and reference speed
Stevie 6:a44d6f7626f2 110 integral[i] = integral[i] + speed_error;
Stevie 6:a44d6f7626f2 111 m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
Stevie 6:a44d6f7626f2 112 }
Stevie 6:a44d6f7626f2 113 if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
Stevie 6:a44d6f7626f2 114 m_duty[0] = 0;
Stevie 6:a44d6f7626f2 115 if(m_speed_ref[1] == 0)
Stevie 6:a44d6f7626f2 116 m_duty[1] = 0;
Stevie 6:a44d6f7626f2 117 }
Stevie 6:a44d6f7626f2 118 else if(control_mode == false) { //distance x and max speed y control
Stevie 6:a44d6f7626f2 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 6:a44d6f7626f2 121 m_duty[i] = Kp_d*distance_error + Ki_d*integral_d[i]; //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 6:a44d6f7626f2 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 2:00535b62c344 233 }
Stevie 2:00535b62c344 234 else if(v == char('D')){
Stevie 2:00535b62c344 235 rpi.reply(0x02);
Stevie 4:abf0070897ff 236 for (i=0;i<=1;i++){
Stevie 6:a44d6f7626f2 237 m_distance_ref[i] = rpi.read() - 128;
Stevie 6:a44d6f7626f2 238 rpi.reply(m_distance_ref[i]);
Stevie 6:a44d6f7626f2 239 m_distance_ref[i] = m_distance_ref[i] * distance_scale;
Stevie 2:00535b62c344 240 }
Stevie 4:abf0070897ff 241 for (i=0;i<=1;i++){
Stevie 6:a44d6f7626f2 242 m_max_duty[i] = rpi.read() - 128;
Stevie 6:a44d6f7626f2 243 rpi.reply(m_max_duty[i]);
Stevie 0:d665d69947ab 244 }
Stevie 2:00535b62c344 245 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 246 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 247 control_mode = false; //set the controller go do distance control
Stevie 6:a44d6f7626f2 248 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 249 m_count[1] = 0;
Stevie 2:00535b62c344 250 }
Stevie 2:00535b62c344 251 else if(v == char('A')){
Stevie 2:00535b62c344 252 rpi.reply(0x03);
Stevie 2:00535b62c344 253 max_accel = rpi.read();
Stevie 2:00535b62c344 254 rpi.reply(max_accel);
Stevie 2:00535b62c344 255 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 256 rpi.reply(0x00);
Stevie 2:00535b62c344 257 }
Stevie 2:00535b62c344 258 else if(v == char('V')){
Stevie 2:00535b62c344 259 rpi.reply(0x04);
Stevie 2:00535b62c344 260 v = rpi.read();
Stevie 2:00535b62c344 261 if(v <= 6){ //check the ADC to be addressed exists
Stevie 2:00535b62c344 262 rpi.reply(adc_values[v]);
Stevie 0:d665d69947ab 263 }
Stevie 2:00535b62c344 264 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 265 rpi.reply(0x00);
Stevie 0:d665d69947ab 266 }
Stevie 0:d665d69947ab 267 }
Stevie 3:497ac7d026b5 268 }
Stevie 4:abf0070897ff 269 void mrfEncoderIsr1(){
Stevie 4:abf0070897ff 270 if(MRF_ENC2 == 0) {
Stevie 3:497ac7d026b5 271 m_count[0] ++;
Stevie 3:497ac7d026b5 272 } else {
Stevie 3:497ac7d026b5 273 m_count[0] --;
Stevie 3:497ac7d026b5 274 }
Stevie 3:497ac7d026b5 275 }
Stevie 4:abf0070897ff 276 void mrfEncoderIsr2(){
Stevie 4:abf0070897ff 277 if(MRF_ENC1 == 1) {
Stevie 4:abf0070897ff 278 m_count[0] ++;
Stevie 3:497ac7d026b5 279 } else {
Stevie 4:abf0070897ff 280 m_count[0] --;
Stevie 3:497ac7d026b5 281 }
Stevie 3:497ac7d026b5 282 }
Stevie 4:abf0070897ff 283 void mlfEncoderIsr1(){
Stevie 4:abf0070897ff 284 if(MLF_ENC2 == 0) {
Stevie 3:497ac7d026b5 285 m_count[1] ++;
Stevie 3:497ac7d026b5 286 } else {
Stevie 3:497ac7d026b5 287 m_count[1] --;
Stevie 3:497ac7d026b5 288 }
Stevie 3:497ac7d026b5 289 }
Stevie 4:abf0070897ff 290 void mlfEncoderIsr2(){
Stevie 4:abf0070897ff 291 if(MLF_ENC1 == 1) {
Stevie 4:abf0070897ff 292 m_count[1] ++;
Stevie 3:497ac7d026b5 293 } else {
Stevie 4:abf0070897ff 294 m_count[1] --;
Stevie 3:497ac7d026b5 295 }
Stevie 3:497ac7d026b5 296 }
Stevie 5:600c5c9cbb19 297
Stevie 5:600c5c9cbb19 298 void controlTick()
Stevie 5:600c5c9cbb19 299 {
Stevie 5:600c5c9cbb19 300 control_loop_flag = true;
Stevie 5:600c5c9cbb19 301 }