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:b6d1ab1ace48, 2018-11-30 (annotated)
- Committer:
- Pabs44
- Date:
- Fri Nov 30 13:34:17 2018 +0000
- Revision:
- 0:b6d1ab1ace48
Test code for class
Who changed what in which revision?
User | Revision | Line number | New 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 | } |