the coding of the bots / Mbed 2 deprecated expanison3

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@1:0d3f572cbc19, 2018-12-03 (annotated)

Committer:
Pabs44
Date:
Mon Dec 03 11:30:12 2018 +0000
Revision:
1:0d3f572cbc19
Parent:
0:e4566f96ab5c 
expansion 3

Who changed what in which revision?

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