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@3:497ac7d026b5, 2019-02-24 (annotated)

Committer:
Stevie
Date:
Sun Feb 24 11:32:05 2019 +0000
Revision:
3:497ac7d026b5
Parent:
2:00535b62c344
Child:
4:abf0070897ff
Some encoder functionality working. Pre-simplification to left/right control

Who changed what in which revision?

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