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

Committer:
Stevie
Date:
Sun Feb 24 12:00:49 2019 +0000
Revision:
4:abf0070897ff
Parent:
3:497ac7d026b5
Child:
5:600c5c9cbb19
Working left/right control with +/- duty cycle control implemented.

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