the coding of the bots / Mbed 2 deprecated buggyboi_expansion2

Dependencies:   mbed

Committer:
Pabs44
Date:
Fri Nov 30 13:34:17 2018 +0000
Revision:
0:b6d1ab1ace48
Test code for class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pabs44 0:b6d1ab1ace48 1 #include "mbed.h"
Pabs44 0:b6d1ab1ace48 2
Pabs44 0:b6d1ab1ace48 3 //Motor PWM (speed)
Pabs44 0:b6d1ab1ace48 4 PwmOut PWMA(PA_8);
Pabs44 0:b6d1ab1ace48 5 PwmOut PWMB(PB_4);
Pabs44 0:b6d1ab1ace48 6 //Motor Direction
Pabs44 0:b6d1ab1ace48 7 DigitalOut DIRA(PA_9);
Pabs44 0:b6d1ab1ace48 8 DigitalOut DIRB(PB_10);
Pabs44 0:b6d1ab1ace48 9 //Hall-Effect Sensor Input
Pabs44 0:b6d1ab1ace48 10 DigitalIn HEA1(PB_2);
Pabs44 0:b6d1ab1ace48 11 DigitalIn HEA2(PB_1);
Pabs44 0:b6d1ab1ace48 12 DigitalIn HEB1(PB_15);
Pabs44 0:b6d1ab1ace48 13 DigitalIn HEB2(PB_14);
Pabs44 0:b6d1ab1ace48 14 //Use the serial object so we can use higher speeds
Pabs44 0:b6d1ab1ace48 15 Serial terminal(USBTX, USBRX);
Pabs44 0:b6d1ab1ace48 16 //Enumerated types
Pabs44 0:b6d1ab1ace48 17 enum DIRECTION {FORWARD=0, REVERSE};
Pabs44 0:b6d1ab1ace48 18 enum PULSE {NOPULSE=0, PULSE};
Pabs44 0:b6d1ab1ace48 19 //Counter Set
Pabs44 0:b6d1ab1ace48 20 int cA1 = 0;
Pabs44 0:b6d1ab1ace48 21 int cB1 = 0;
Pabs44 0:b6d1ab1ace48 22 int cws = 0;
Pabs44 0:b6d1ab1ace48 23 //Timer Set
Pabs44 0:b6d1ab1ace48 24 Timer timer;
Pabs44 0:b6d1ab1ace48 25 //Set Duty Cycles
Pabs44 0:b6d1ab1ace48 26 float dutyA = 1.0f;
Pabs44 0:b6d1ab1ace48 27 float dutyB = 1.0f;
Pabs44 0:b6d1ab1ace48 28 float dA;
Pabs44 0:b6d1ab1ace48 29 float dB;
Pabs44 0:b6d1ab1ace48 30
Pabs44 0:b6d1ab1ace48 31 //Pulse Counter for wheel A
Pabs44 0:b6d1ab1ace48 32 void pulsecountA(){
Pabs44 0:b6d1ab1ace48 33 while(HEA1 == PULSE);
Pabs44 0:b6d1ab1ace48 34 while(HEA1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 35 while(HEA1 == PULSE);
Pabs44 0:b6d1ab1ace48 36 while(HEA1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 37 cA1 = cA1 + 1;
Pabs44 0:b6d1ab1ace48 38 cws = cws + 1;
Pabs44 0:b6d1ab1ace48 39 }
Pabs44 0:b6d1ab1ace48 40 //Pulse Counter for wheel B
Pabs44 0:b6d1ab1ace48 41 void pulsecountB(){
Pabs44 0:b6d1ab1ace48 42 while(HEB1 == PULSE);
Pabs44 0:b6d1ab1ace48 43 while(HEB1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 44 while(HEB1 == PULSE);
Pabs44 0:b6d1ab1ace48 45 while(HEB1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 46 cB1 = cB1 + 1;
Pabs44 0:b6d1ab1ace48 47 }
Pabs44 0:b6d1ab1ace48 48
Pabs44 0:b6d1ab1ace48 49 //Speed calculation for wheel A
Pabs44 0:b6d1ab1ace48 50 void wsA(){
Pabs44 0:b6d1ab1ace48 51 int tA1[2];
Pabs44 0:b6d1ab1ace48 52 int tA2[2];
Pabs44 0:b6d1ab1ace48 53
Pabs44 0:b6d1ab1ace48 54 while(HEA1 == PULSE);
Pabs44 0:b6d1ab1ace48 55
Pabs44 0:b6d1ab1ace48 56 while(HEA1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 57 tA1[0] = timer.read_us();
Pabs44 0:b6d1ab1ace48 58 while(HEA2 == NOPULSE);
Pabs44 0:b6d1ab1ace48 59 tA2[0] = timer.read_us();
Pabs44 0:b6d1ab1ace48 60
Pabs44 0:b6d1ab1ace48 61 while(HEA1 == PULSE);
Pabs44 0:b6d1ab1ace48 62 while(HEA2 == PULSE);
Pabs44 0:b6d1ab1ace48 63
Pabs44 0:b6d1ab1ace48 64 while(HEA1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 65 tA1[1] = timer.read_us();
Pabs44 0:b6d1ab1ace48 66 while(HEA2 == NOPULSE);
Pabs44 0:b6d1ab1ace48 67 tA2[1] = timer.read_us();
Pabs44 0:b6d1ab1ace48 68
Pabs44 0:b6d1ab1ace48 69 float fA1 = 1.0f/((tA1[1]-tA1[0])*(float)3.0E-6);
Pabs44 0:b6d1ab1ace48 70 float fA2 = 1.0f/((tA2[1]-tA2[0])*(float)3.0E-6);
Pabs44 0:b6d1ab1ace48 71
Pabs44 0:b6d1ab1ace48 72 float avA = (fA1 + fA2)/2.0f;
Pabs44 0:b6d1ab1ace48 73
Pabs44 0:b6d1ab1ace48 74 float wsA = avA/20.8f;
Pabs44 0:b6d1ab1ace48 75
Pabs44 0:b6d1ab1ace48 76 terminal.printf("\n\rA Wheel Speed = %6.2f", wsA);
Pabs44 0:b6d1ab1ace48 77
Pabs44 0:b6d1ab1ace48 78 dA = 1.0f - wsA;
Pabs44 0:b6d1ab1ace48 79 }
Pabs44 0:b6d1ab1ace48 80 //Speed calculation for wheel B
Pabs44 0:b6d1ab1ace48 81 void wsB(){
Pabs44 0:b6d1ab1ace48 82 int tB1[2];
Pabs44 0:b6d1ab1ace48 83 int tB2[2];
Pabs44 0:b6d1ab1ace48 84
Pabs44 0:b6d1ab1ace48 85 while(HEB1 == PULSE);
Pabs44 0:b6d1ab1ace48 86
Pabs44 0:b6d1ab1ace48 87 while(HEB1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 88 tB1[0] = timer.read_us();
Pabs44 0:b6d1ab1ace48 89 while(HEB2 == NOPULSE);
Pabs44 0:b6d1ab1ace48 90 tB2[0] = timer.read_us();
Pabs44 0:b6d1ab1ace48 91
Pabs44 0:b6d1ab1ace48 92 while(HEB1 == PULSE);
Pabs44 0:b6d1ab1ace48 93 while(HEB2 == PULSE);
Pabs44 0:b6d1ab1ace48 94
Pabs44 0:b6d1ab1ace48 95 while(HEB1 == NOPULSE);
Pabs44 0:b6d1ab1ace48 96 tB1[1] = timer.read_us();
Pabs44 0:b6d1ab1ace48 97 while(HEB2 == NOPULSE);
Pabs44 0:b6d1ab1ace48 98 tB2[1] = timer.read_us();
Pabs44 0:b6d1ab1ace48 99
Pabs44 0:b6d1ab1ace48 100 float fB1 = 1.0f/((tB1[1]-tB1[0])*(float)3.0E-6);
Pabs44 0:b6d1ab1ace48 101 float fB2 = 1.0f/((tB2[1]-tB2[0])*(float)3.0E-6);
Pabs44 0:b6d1ab1ace48 102
Pabs44 0:b6d1ab1ace48 103 float avB = (fB1 + fB2)/2.0f;
Pabs44 0:b6d1ab1ace48 104
Pabs44 0:b6d1ab1ace48 105 float wsB = avB/20.8f;
Pabs44 0:b6d1ab1ace48 106
Pabs44 0:b6d1ab1ace48 107 terminal.printf("\n\rB Wheel Speed = %6.2f", wsB);
Pabs44 0:b6d1ab1ace48 108
Pabs44 0:b6d1ab1ace48 109 dB = 1.0f - wsB;
Pabs44 0:b6d1ab1ace48 110 }
Pabs44 0:b6d1ab1ace48 111
Pabs44 0:b6d1ab1ace48 112 //Calculate Duty Cycle Correction
Pabs44 0:b6d1ab1ace48 113 void correction(){
Pabs44 0:b6d1ab1ace48 114 dutyA = dutyA + dA*0.1f;
Pabs44 0:b6d1ab1ace48 115 dutyB = dutyB + dB*0.1f;
Pabs44 0:b6d1ab1ace48 116 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Pabs44 0:b6d1ab1ace48 117 dutyA = (dutyA<0.01f) ? 0.01f : dutyA;
Pabs44 0:b6d1ab1ace48 118 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Pabs44 0:b6d1ab1ace48 119 dutyB = (dutyB<0.01f) ? 0.01f : dutyB;
Pabs44 0:b6d1ab1ace48 120 PWMA.write(dutyA);
Pabs44 0:b6d1ab1ace48 121 PWMB.write(dutyB);
Pabs44 0:b6d1ab1ace48 122 terminal.printf("\n\rDuty A = %6.2f", dutyA);
Pabs44 0:b6d1ab1ace48 123 terminal.printf("\n\rDuty B = %6.2f", dutyB);
Pabs44 0:b6d1ab1ace48 124 }
Pabs44 0:b6d1ab1ace48 125
Pabs44 0:b6d1ab1ace48 126 //Movement Instructions
Pabs44 0:b6d1ab1ace48 127 //Drive function
Pabs44 0:b6d1ab1ace48 128 void drive(){
Pabs44 0:b6d1ab1ace48 129 PWMA.write(dutyA);
Pabs44 0:b6d1ab1ace48 130 PWMB.write(dutyB);
Pabs44 0:b6d1ab1ace48 131 }
Pabs44 0:b6d1ab1ace48 132 //Turn function
Pabs44 0:b6d1ab1ace48 133 void turn(){
Pabs44 0:b6d1ab1ace48 134 PWMA.write(dutyA);
Pabs44 0:b6d1ab1ace48 135 PWMB.write(0.0f);
Pabs44 0:b6d1ab1ace48 136 }
Pabs44 0:b6d1ab1ace48 137 //Victory dance function
Pabs44 0:b6d1ab1ace48 138 void victoryDance(){
Pabs44 0:b6d1ab1ace48 139 PWMA.write(dutyA/4);
Pabs44 0:b6d1ab1ace48 140 PWMB.write(dutyB/4);
Pabs44 0:b6d1ab1ace48 141 DIRA = FORWARD;
Pabs44 0:b6d1ab1ace48 142 DIRB = REVERSE;
Pabs44 0:b6d1ab1ace48 143 }
Pabs44 0:b6d1ab1ace48 144
Pabs44 0:b6d1ab1ace48 145 //Start code------------------------------------
Pabs44 0:b6d1ab1ace48 146 int main() {
Pabs44 0:b6d1ab1ace48 147 //Setup
Pabs44 0:b6d1ab1ace48 148 terminal.baud(115200);
Pabs44 0:b6d1ab1ace48 149 PWMA.period_ms(10);
Pabs44 0:b6d1ab1ace48 150 PWMB.period_ms(10);
Pabs44 0:b6d1ab1ace48 151 DIRA = FORWARD;
Pabs44 0:b6d1ab1ace48 152 DIRB = FORWARD;
Pabs44 0:b6d1ab1ace48 153
Pabs44 0:b6d1ab1ace48 154 //Start
Pabs44 0:b6d1ab1ace48 155 wait(2);
Pabs44 0:b6d1ab1ace48 156 terminal.printf("\n\rNew Test\n\r");
Pabs44 0:b6d1ab1ace48 157 timer.reset();
Pabs44 0:b6d1ab1ace48 158 timer.start();
Pabs44 0:b6d1ab1ace48 159 drive();
Pabs44 0:b6d1ab1ace48 160
Pabs44 0:b6d1ab1ace48 161 //Move 177 pulses (1 meter)-----------------
Pabs44 0:b6d1ab1ace48 162 while(cA1 < 177 && cB1 < 177){
Pabs44 0:b6d1ab1ace48 163 drive();
Pabs44 0:b6d1ab1ace48 164 pulsecountA();
Pabs44 0:b6d1ab1ace48 165 pulsecountB();
Pabs44 0:b6d1ab1ace48 166 if(cws == 4){
Pabs44 0:b6d1ab1ace48 167 wsA();
Pabs44 0:b6d1ab1ace48 168 wsB();
Pabs44 0:b6d1ab1ace48 169 correction();
Pabs44 0:b6d1ab1ace48 170 cws = 0;
Pabs44 0:b6d1ab1ace48 171 }
Pabs44 0:b6d1ab1ace48 172 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 173 terminal.printf("\n\rB Pulses = %d", cB1);
Pabs44 0:b6d1ab1ace48 174 }
Pabs44 0:b6d1ab1ace48 175 cA1 = 0;
Pabs44 0:b6d1ab1ace48 176 cB1 = 0;
Pabs44 0:b6d1ab1ace48 177
Pabs44 0:b6d1ab1ace48 178 //Move 24 pulses (turn)---------------------
Pabs44 0:b6d1ab1ace48 179 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 180 while (cA1 < 24){
Pabs44 0:b6d1ab1ace48 181 turn();
Pabs44 0:b6d1ab1ace48 182 pulsecountA();
Pabs44 0:b6d1ab1ace48 183 if(cws == 4){
Pabs44 0:b6d1ab1ace48 184 wsA();
Pabs44 0:b6d1ab1ace48 185 cws = 0;
Pabs44 0:b6d1ab1ace48 186 }
Pabs44 0:b6d1ab1ace48 187 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 188 }
Pabs44 0:b6d1ab1ace48 189 cA1 = 0;
Pabs44 0:b6d1ab1ace48 190 cB1 = 0;
Pabs44 0:b6d1ab1ace48 191
Pabs44 0:b6d1ab1ace48 192 //Move 89 pulses (0.5 meters)---------------
Pabs44 0:b6d1ab1ace48 193 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 194 while(cA1 < 89 && cB1 < 89){
Pabs44 0:b6d1ab1ace48 195 drive();
Pabs44 0:b6d1ab1ace48 196 pulsecountA();
Pabs44 0:b6d1ab1ace48 197 pulsecountB();
Pabs44 0:b6d1ab1ace48 198 if(cws == 4){
Pabs44 0:b6d1ab1ace48 199 wsA();
Pabs44 0:b6d1ab1ace48 200 wsB();
Pabs44 0:b6d1ab1ace48 201 correction();
Pabs44 0:b6d1ab1ace48 202 cws = 0;
Pabs44 0:b6d1ab1ace48 203 }
Pabs44 0:b6d1ab1ace48 204 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 205 terminal.printf("\n\rB Pulses = %d", cB1);
Pabs44 0:b6d1ab1ace48 206 }
Pabs44 0:b6d1ab1ace48 207 cA1 = 0;
Pabs44 0:b6d1ab1ace48 208 cB1 = 0;
Pabs44 0:b6d1ab1ace48 209
Pabs44 0:b6d1ab1ace48 210 //Move 24 pulses (turn)---------------------
Pabs44 0:b6d1ab1ace48 211 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 212 while (cA1 < 24){
Pabs44 0:b6d1ab1ace48 213 turn();
Pabs44 0:b6d1ab1ace48 214 pulsecountA();
Pabs44 0:b6d1ab1ace48 215 if(cws == 4){
Pabs44 0:b6d1ab1ace48 216 wsA();
Pabs44 0:b6d1ab1ace48 217 cws = 0;
Pabs44 0:b6d1ab1ace48 218 }
Pabs44 0:b6d1ab1ace48 219 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 220 }
Pabs44 0:b6d1ab1ace48 221 cA1 = 0;
Pabs44 0:b6d1ab1ace48 222 cB1 = 0;
Pabs44 0:b6d1ab1ace48 223
Pabs44 0:b6d1ab1ace48 224 //Move 177 pulses (1 meter)-----------------
Pabs44 0:b6d1ab1ace48 225 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 226 while(cA1 < 177 && cB1 < 177){
Pabs44 0:b6d1ab1ace48 227 drive();
Pabs44 0:b6d1ab1ace48 228 pulsecountA();
Pabs44 0:b6d1ab1ace48 229 pulsecountB();
Pabs44 0:b6d1ab1ace48 230 if(cws == 4){
Pabs44 0:b6d1ab1ace48 231 wsA();
Pabs44 0:b6d1ab1ace48 232 wsB();
Pabs44 0:b6d1ab1ace48 233 correction();
Pabs44 0:b6d1ab1ace48 234 cws = 0;
Pabs44 0:b6d1ab1ace48 235 }
Pabs44 0:b6d1ab1ace48 236 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 237 terminal.printf("\n\rB Pulses = %d", cB1);
Pabs44 0:b6d1ab1ace48 238 }
Pabs44 0:b6d1ab1ace48 239 cA1 = 0;
Pabs44 0:b6d1ab1ace48 240 cB1 = 0;
Pabs44 0:b6d1ab1ace48 241
Pabs44 0:b6d1ab1ace48 242 //Move 24 pulses (turn)---------------------
Pabs44 0:b6d1ab1ace48 243 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 244 while (cA1 < 24){
Pabs44 0:b6d1ab1ace48 245 turn();
Pabs44 0:b6d1ab1ace48 246 pulsecountA();
Pabs44 0:b6d1ab1ace48 247 if(cws == 4){
Pabs44 0:b6d1ab1ace48 248 wsA();
Pabs44 0:b6d1ab1ace48 249 cws = 0;
Pabs44 0:b6d1ab1ace48 250 }
Pabs44 0:b6d1ab1ace48 251 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 252 }
Pabs44 0:b6d1ab1ace48 253 cA1 = 0;
Pabs44 0:b6d1ab1ace48 254 cB1 = 0;
Pabs44 0:b6d1ab1ace48 255
Pabs44 0:b6d1ab1ace48 256 //Move 89 pulses (0.5 meters)---------------
Pabs44 0:b6d1ab1ace48 257 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 258 while(cA1 < 89 && cB1 < 89){
Pabs44 0:b6d1ab1ace48 259 drive();
Pabs44 0:b6d1ab1ace48 260 pulsecountA();
Pabs44 0:b6d1ab1ace48 261 pulsecountB();
Pabs44 0:b6d1ab1ace48 262 if(cws == 4){
Pabs44 0:b6d1ab1ace48 263 wsA();
Pabs44 0:b6d1ab1ace48 264 wsB();
Pabs44 0:b6d1ab1ace48 265 correction();
Pabs44 0:b6d1ab1ace48 266 cws = 0;
Pabs44 0:b6d1ab1ace48 267 }
Pabs44 0:b6d1ab1ace48 268 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 269 terminal.printf("\n\rB Pulses = %d", cB1);
Pabs44 0:b6d1ab1ace48 270 }
Pabs44 0:b6d1ab1ace48 271 cA1 = 0;
Pabs44 0:b6d1ab1ace48 272 cB1 = 0;
Pabs44 0:b6d1ab1ace48 273
Pabs44 0:b6d1ab1ace48 274 //Move 24 pulses (turn)---------------------
Pabs44 0:b6d1ab1ace48 275 terminal.printf("\n\r");
Pabs44 0:b6d1ab1ace48 276 while (cA1 < 24){
Pabs44 0:b6d1ab1ace48 277 turn();
Pabs44 0:b6d1ab1ace48 278 pulsecountA();
Pabs44 0:b6d1ab1ace48 279 if(cws == 4){
Pabs44 0:b6d1ab1ace48 280 wsA();
Pabs44 0:b6d1ab1ace48 281 cws = 0;
Pabs44 0:b6d1ab1ace48 282 }
Pabs44 0:b6d1ab1ace48 283 terminal.printf("\n\rA Pulses = %d", cA1);
Pabs44 0:b6d1ab1ace48 284 }
Pabs44 0:b6d1ab1ace48 285 cA1 = 0;
Pabs44 0:b6d1ab1ace48 286 cB1 = 0;
Pabs44 0:b6d1ab1ace48 287
Pabs44 0:b6d1ab1ace48 288 //Vicorty Dance-----------------------------
Pabs44 0:b6d1ab1ace48 289 victoryDance();
Pabs44 0:b6d1ab1ace48 290 wait(3);
Pabs44 0:b6d1ab1ace48 291
Pabs44 0:b6d1ab1ace48 292 //Finish------------------------------------
Pabs44 0:b6d1ab1ace48 293 PWMA.write(0.0f);
Pabs44 0:b6d1ab1ace48 294 PWMB.write(0.0f);
Pabs44 0:b6d1ab1ace48 295 }