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@5:600c5c9cbb19, 2019-02-24 (annotated)

Committer:
Stevie
Date:
Sun Feb 24 12:29:11 2019 +0000
Revision:
5:600c5c9cbb19
Parent:
4:abf0070897ff
Child:
6:a44d6f7626f2
Working left/right closed loop speed control. Not yet tuned for SKyFawkes.

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 4:abf0070897ff 42 float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
Stevie 5:600c5c9cbb19 43 int m_distance_ref [2] = {0,0}; //setup array for 2 motor distances
Stevie 0:d665d69947ab 44 int max_accel = 0; //for storing the maximum acceleration
Stevie 1:08315c315df0 45 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
Stevie 0:d665d69947ab 46
Stevie 2:00535b62c344 47 void readADC(); //read 6 channels of ADC
Stevie 2:00535b62c344 48 void setDuty(); //set the 4 motor PWM duty cycles
Stevie 2:00535b62c344 49 void setPeriod(int period); //set PWM period for motors in microseconds
Stevie 2:00535b62c344 50 void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 51 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 52 void spiComms(); //do spi communications with raspberry pi
Stevie 2:00535b62c344 53
Stevie 3:497ac7d026b5 54 void mrfEncoderIsr1();
Stevie 3:497ac7d026b5 55 void mrfEncoderIsr2();
Stevie 3:497ac7d026b5 56 void mlfEncoderIsr1();
Stevie 3:497ac7d026b5 57 void mlfEncoderIsr2();
Stevie 5:600c5c9cbb19 58 void controlTick();
Stevie 3:497ac7d026b5 59
Stevie 0:d665d69947ab 60
Stevie 0:d665d69947ab 61 int main()
Stevie 0:d665d69947ab 62 {
Stevie 5:600c5c9cbb19 63 float pos_change = 0; //temp variable for speed control
Stevie 5:600c5c9cbb19 64 float speed_error = 0; //temp variable for speed control
Stevie 5:600c5c9cbb19 65 float integral [2] = {0,0};
Stevie 5:600c5c9cbb19 66 float Kp = 0.01; //proportional constant
Stevie 5:600c5c9cbb19 67 float Ki = 0.01; //integral constant
Stevie 5:600c5c9cbb19 68 int i = 0;
Stevie 5:600c5c9cbb19 69
Stevie 5:600c5c9cbb19 70
Stevie 3:497ac7d026b5 71 //setup interrupts for encoders
Stevie 3:497ac7d026b5 72 MRF_ENC1.fall(&mrfEncoderIsr1);
Stevie 3:497ac7d026b5 73 MRF_ENC2.fall(&mrfEncoderIsr2);
Stevie 3:497ac7d026b5 74 MLF_ENC1.fall(&mlfEncoderIsr1);
Stevie 3:497ac7d026b5 75 MLF_ENC2.fall(&mlfEncoderIsr2);
Stevie 3:497ac7d026b5 76
Stevie 0:d665d69947ab 77 //setup driver pins
Stevie 2:00535b62c344 78 setLMotorDir(1);
Stevie 2:00535b62c344 79 setRMotorDir(1);
Stevie 2:00535b62c344 80 // Set PWM period in us
Stevie 2:00535b62c344 81 setPeriod(100);
Stevie 4:abf0070897ff 82 // pc.printf("Starting up");
Stevie 5:600c5c9cbb19 83 //setup control loop
Stevie 5:600c5c9cbb19 84 control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms
Stevie 5:600c5c9cbb19 85
Stevie 5:600c5c9cbb19 86
Stevie 3:497ac7d026b5 87
Stevie 0:d665d69947ab 88 while (1) {
Stevie 4:abf0070897ff 89 // pc.printf("Motor count %i\r\n", m_count[0]);
Stevie 5:600c5c9cbb19 90
Stevie 5:600c5c9cbb19 91 if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
Stevie 5:600c5c9cbb19 92 for(i=0;i<=1;i++){ //do this for right and left in turn
Stevie 5:600c5c9cbb19 93 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
Stevie 5:600c5c9cbb19 94 m_last_count[i] = m_count[i]; //store count for next cycle
Stevie 5:600c5c9cbb19 95 speed_error = pos_change - m_speed_ref[i]; //calculate different between current speed and reference speed
Stevie 5:600c5c9cbb19 96 integral[i] = integral[i] + speed_error;
Stevie 5:600c5c9cbb19 97 m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
Stevie 5:600c5c9cbb19 98 }
Stevie 5:600c5c9cbb19 99
Stevie 5:600c5c9cbb19 100 if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
Stevie 5:600c5c9cbb19 101 m_duty[0] = 0;
Stevie 5:600c5c9cbb19 102 if(m_speed_ref[1] == 0)
Stevie 5:600c5c9cbb19 103 m_duty[1] = 0;
Stevie 5:600c5c9cbb19 104
Stevie 5:600c5c9cbb19 105 if(m_duty[0] < 0){
Stevie 5:600c5c9cbb19 106 setRMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 107 m_duty[0] = -1*m_duty[0]; //make the negative value into positive
Stevie 5:600c5c9cbb19 108 } else {
Stevie 5:600c5c9cbb19 109 setRMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 110 }
Stevie 5:600c5c9cbb19 111 if(m_duty[0] > 100){
Stevie 5:600c5c9cbb19 112 m_duty[0] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 113 }
Stevie 5:600c5c9cbb19 114
Stevie 5:600c5c9cbb19 115 if(m_duty[1] < 0){
Stevie 5:600c5c9cbb19 116 setLMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 117 m_duty[1] = -1*m_duty[1]; //make the negative value into positive
Stevie 5:600c5c9cbb19 118 } else {
Stevie 5:600c5c9cbb19 119 setLMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 120 }
Stevie 5:600c5c9cbb19 121 if(m_duty[1] > 100){
Stevie 5:600c5c9cbb19 122 m_duty[1] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 123 }
Stevie 5:600c5c9cbb19 124 setDuty(); //set all the duty cycles to the motor drivers
Stevie 5:600c5c9cbb19 125 control_loop_flag = false;
Stevie 4:abf0070897ff 126 }
Stevie 4:abf0070897ff 127
Stevie 3:497ac7d026b5 128 readADC(); //read all the ADC values when not doing something else
Stevie 3:497ac7d026b5 129 spiComms(); //do SPI communication stuff
Stevie 2:00535b62c344 130 }
Stevie 2:00535b62c344 131 }
Stevie 2:00535b62c344 132
Stevie 2:00535b62c344 133 //function to read all 6 ADC channels
Stevie 2:00535b62c344 134 void readADC(){
Stevie 2:00535b62c344 135 adc_values[0] = int(ANALOG0.read()*255);
Stevie 2:00535b62c344 136 adc_values[1] = int(ANALOG1.read()*255);
Stevie 2:00535b62c344 137 adc_values[2] = int(ANALOG2.read()*255);
Stevie 2:00535b62c344 138 adc_values[3] = int(ANALOG3.read()*255);
Stevie 2:00535b62c344 139 adc_values[4] = int(ANALOG4.read()*255);
Stevie 2:00535b62c344 140 adc_values[5] = int(ANALOG5.read()*255);
Stevie 2:00535b62c344 141 }
Stevie 2:00535b62c344 142
Stevie 2:00535b62c344 143 //function to set all 4 motor PWM duty cycles
Stevie 2:00535b62c344 144 void setDuty(){
Stevie 2:00535b62c344 145 MRB.write(m_duty[0]);
Stevie 4:abf0070897ff 146 MRF.write(m_duty[0]);
Stevie 4:abf0070897ff 147 MLB.write(m_duty[1]);
Stevie 4:abf0070897ff 148 MLF.write(m_duty[1]);
Stevie 2:00535b62c344 149 }
Stevie 2:00535b62c344 150 //set left motor direction. 1 is forward, 0 is backwards.
Stevie 2:00535b62c344 151 void setLMotorDir(bool direction){
Stevie 2:00535b62c344 152 LSTANDBY = 1;
Stevie 2:00535b62c344 153 if(direction == true){
Stevie 2:00535b62c344 154 LAIN1 = 1;
Stevie 2:00535b62c344 155 LAIN2 = 0;
Stevie 2:00535b62c344 156 }
Stevie 2:00535b62c344 157 else if(direction == false){
Stevie 2:00535b62c344 158 LAIN1 = 0;
Stevie 2:00535b62c344 159 LAIN2 = 1;
Stevie 2:00535b62c344 160 }
Stevie 2:00535b62c344 161 }
Stevie 2:00535b62c344 162 //set right motor direction. 1 is forward, 0 is backwards.
Stevie 2:00535b62c344 163 void setRMotorDir(bool direction){
Stevie 2:00535b62c344 164 RSTANDBY = 1;
Stevie 2:00535b62c344 165 if(direction == true){
Stevie 2:00535b62c344 166 RAIN1 = 0;
Stevie 2:00535b62c344 167 RAIN2 = 1;
Stevie 2:00535b62c344 168 }
Stevie 2:00535b62c344 169 else if(direction == false){
Stevie 2:00535b62c344 170 RAIN1 = 1;
Stevie 2:00535b62c344 171 RAIN2 = 0;
Stevie 2:00535b62c344 172 }
Stevie 2:00535b62c344 173 }
Stevie 2:00535b62c344 174
Stevie 2:00535b62c344 175 //initialisation function to set motor PWM period and set to 0 duty
Stevie 2:00535b62c344 176 void setPeriod(int period){
Stevie 2:00535b62c344 177 MRB.period_us(period);
Stevie 2:00535b62c344 178 MRB.write(0.0);
Stevie 2:00535b62c344 179 MRF.period_us(period);
Stevie 2:00535b62c344 180 MRF.write(0.0);
Stevie 2:00535b62c344 181 MLB.period_us(period);
Stevie 2:00535b62c344 182 MLB.write(0.0);
Stevie 2:00535b62c344 183 MLF.period_us(period);
Stevie 2:00535b62c344 184 MLF.write(0.0);
Stevie 2:00535b62c344 185 }
Stevie 2:00535b62c344 186
Stevie 2:00535b62c344 187 void spiComms(){
Stevie 2:00535b62c344 188 //do SPI communication stuff
Stevie 2:00535b62c344 189 int i = 0; //temp counter variable
Stevie 2:00535b62c344 190 int v = 0; //temp SPI variable
Stevie 2:00535b62c344 191 if(rpi.receive()) {
Stevie 2:00535b62c344 192 v = rpi.read(); // Read byte from master
Stevie 2:00535b62c344 193 if(v == char('S')){
Stevie 2:00535b62c344 194 rpi.reply(0x01);
Stevie 4:abf0070897ff 195 for (i=0;i<=1;i++){
Stevie 2:00535b62c344 196 m_speed_ref[i] = rpi.read() - 128;
Stevie 2:00535b62c344 197 rpi.reply(m_speed_ref[i]);
Stevie 0:d665d69947ab 198 }
Stevie 2:00535b62c344 199 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 200 rpi.reply(0x00);
Stevie 2:00535b62c344 201 }
Stevie 2:00535b62c344 202 else if(v == char('D')){
Stevie 2:00535b62c344 203 rpi.reply(0x02);
Stevie 4:abf0070897ff 204 for (i=0;i<=1;i++){
Stevie 2:00535b62c344 205 m_speed_ref[i] = rpi.read() - 128;
Stevie 2:00535b62c344 206 rpi.reply(m_speed_ref[i]);
Stevie 2:00535b62c344 207 }
Stevie 4:abf0070897ff 208 for (i=0;i<=1;i++){
Stevie 2:00535b62c344 209 m_distance_ref[i] = rpi.read() - 128;
Stevie 2:00535b62c344 210 rpi.reply(m_distance_ref[i]);
Stevie 0:d665d69947ab 211 }
Stevie 2:00535b62c344 212 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 213 rpi.reply(0x00);
Stevie 2:00535b62c344 214 }
Stevie 2:00535b62c344 215 else if(v == char('A')){
Stevie 2:00535b62c344 216 rpi.reply(0x03);
Stevie 2:00535b62c344 217 max_accel = rpi.read();
Stevie 2:00535b62c344 218 rpi.reply(max_accel);
Stevie 2:00535b62c344 219 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 220 rpi.reply(0x00);
Stevie 2:00535b62c344 221 }
Stevie 2:00535b62c344 222 else if(v == char('V')){
Stevie 2:00535b62c344 223 rpi.reply(0x04);
Stevie 2:00535b62c344 224 v = rpi.read();
Stevie 2:00535b62c344 225 if(v <= 6){ //check the ADC to be addressed exists
Stevie 2:00535b62c344 226 rpi.reply(adc_values[v]);
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 0:d665d69947ab 230 }
Stevie 0:d665d69947ab 231 }
Stevie 3:497ac7d026b5 232 }
Stevie 4:abf0070897ff 233 void mrfEncoderIsr1(){
Stevie 4:abf0070897ff 234 if(MRF_ENC2 == 0) {
Stevie 3:497ac7d026b5 235 m_count[0] ++;
Stevie 3:497ac7d026b5 236 } else {
Stevie 3:497ac7d026b5 237 m_count[0] --;
Stevie 3:497ac7d026b5 238 }
Stevie 3:497ac7d026b5 239 }
Stevie 4:abf0070897ff 240 void mrfEncoderIsr2(){
Stevie 4:abf0070897ff 241 if(MRF_ENC1 == 1) {
Stevie 4:abf0070897ff 242 m_count[0] ++;
Stevie 3:497ac7d026b5 243 } else {
Stevie 4:abf0070897ff 244 m_count[0] --;
Stevie 3:497ac7d026b5 245 }
Stevie 3:497ac7d026b5 246 }
Stevie 4:abf0070897ff 247 void mlfEncoderIsr1(){
Stevie 4:abf0070897ff 248 if(MLF_ENC2 == 0) {
Stevie 3:497ac7d026b5 249 m_count[1] ++;
Stevie 3:497ac7d026b5 250 } else {
Stevie 3:497ac7d026b5 251 m_count[1] --;
Stevie 3:497ac7d026b5 252 }
Stevie 3:497ac7d026b5 253 }
Stevie 4:abf0070897ff 254 void mlfEncoderIsr2(){
Stevie 4:abf0070897ff 255 if(MLF_ENC1 == 1) {
Stevie 4:abf0070897ff 256 m_count[1] ++;
Stevie 3:497ac7d026b5 257 } else {
Stevie 4:abf0070897ff 258 m_count[1] --;
Stevie 3:497ac7d026b5 259 }
Stevie 3:497ac7d026b5 260 }
Stevie 5:600c5c9cbb19 261
Stevie 5:600c5c9cbb19 262 void controlTick()
Stevie 5:600c5c9cbb19 263 {
Stevie 5:600c5c9cbb19 264 control_loop_flag = true;
Stevie 5:600c5c9cbb19 265 }