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@0:e4566f96ab5c, 2018-11-30 (annotated)

Committer:
joefish113
Date:
Fri Nov 30 13:41:21 2018 +0000
Revision:
0:e4566f96ab5c
Child:
1:0d3f572cbc19
buggyboi

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