PIDs and fun
Dependencies: btbee m3pi_ng mbed FatFileSystemCpp
main.cpp@8:e7b322f72d16, 2015-05-28 (annotated)
- Committer:
- crmackey
- Date:
- Thu May 28 12:56:20 2015 +0000
- Revision:
- 8:e7b322f72d16
- Parent:
- 7:74c9a0fbf2b1
- Child:
- 9:43be0dea44f0
Made the robot stop at the end of the line
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
crmackey | 0:4d1ae7eb0070 | 1 | #include "mbed.h" |
crmackey | 0:4d1ae7eb0070 | 2 | #include "m3pi_ng.h" |
crmackey | 0:4d1ae7eb0070 | 3 | #include "time.h" |
crmackey | 0:4d1ae7eb0070 | 4 | #include "btbee.h" |
crmackey | 0:4d1ae7eb0070 | 5 | |
crmackey | 0:4d1ae7eb0070 | 6 | m3pi m3pi; |
crmackey | 0:4d1ae7eb0070 | 7 | btbee btbee; |
crmackey | 0:4d1ae7eb0070 | 8 | DigitalIn m3pi_pb(p21); |
crmackey | 1:936e329e840a | 9 | DigitalIn m3pi_IN[] = {(p12)}; |
crmackey | 0:4d1ae7eb0070 | 10 | DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)}; |
crmackey | 0:4d1ae7eb0070 | 11 | DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)}; |
crmackey | 0:4d1ae7eb0070 | 12 | |
crmackey | 0:4d1ae7eb0070 | 13 | // Minimum and maximum motor speeds |
crmackey | 0:4d1ae7eb0070 | 14 | #define MAX 1.0 |
crmackey | 0:4d1ae7eb0070 | 15 | #define MIN 0 |
crmackey | 0:4d1ae7eb0070 | 16 | |
crmackey | 0:4d1ae7eb0070 | 17 | // PID terms |
crmackey | 6:75a6f6b50188 | 18 | double P_TERM = 1.5; |
crmackey | 6:75a6f6b50188 | 19 | double I_TERM = 0; |
crmackey | 6:75a6f6b50188 | 20 | double D_TERM = 25; |
crmackey | 6:75a6f6b50188 | 21 | char arr_read[30]; |
crmackey | 6:75a6f6b50188 | 22 | int chars_read; |
crmackey | 0:4d1ae7eb0070 | 23 | |
crmackey | 0:4d1ae7eb0070 | 24 | int main() { |
crmackey | 0:4d1ae7eb0070 | 25 | |
crmackey | 1:936e329e840a | 26 | |
crmackey | 0:4d1ae7eb0070 | 27 | |
crmackey | 0:4d1ae7eb0070 | 28 | btbee.reset(); |
crmackey | 0:4d1ae7eb0070 | 29 | m3pi_pb.mode(PullUp); |
crmackey | 0:4d1ae7eb0070 | 30 | |
crmackey | 0:4d1ae7eb0070 | 31 | m3pi.printf("Wait 4"); |
crmackey | 0:4d1ae7eb0070 | 32 | m3pi.locate(0,1); |
crmackey | 0:4d1ae7eb0070 | 33 | m3pi.printf("PC"); |
crmackey | 0:4d1ae7eb0070 | 34 | |
crmackey | 0:4d1ae7eb0070 | 35 | while(m3pi_pb) { |
crmackey | 0:4d1ae7eb0070 | 36 | m3pi_led[0]=!m3pi_led[0]; |
crmackey | 0:4d1ae7eb0070 | 37 | wait(3); |
crmackey | 0:4d1ae7eb0070 | 38 | btbee.printf("\n"); |
crmackey | 0:4d1ae7eb0070 | 39 | btbee.printf("PC connected. Press the button \n"); |
crmackey | 0:4d1ae7eb0070 | 40 | } |
crmackey | 0:4d1ae7eb0070 | 41 | |
crmackey | 1:936e329e840a | 42 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 43 | |
crmackey | 0:4d1ae7eb0070 | 44 | Timer LapTimer; |
crmackey | 5:3f29267b18e3 | 45 | Timer LoopTime; |
crmackey | 0:4d1ae7eb0070 | 46 | char Bat[] = {'V','o','l','t',' ','i','s'}; |
crmackey | 0:4d1ae7eb0070 | 47 | m3pi.print(Bat,7); |
crmackey | 0:4d1ae7eb0070 | 48 | wait(0.75); |
crmackey | 0:4d1ae7eb0070 | 49 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 50 | float batteryvoltage = m3pi.battery(); |
crmackey | 0:4d1ae7eb0070 | 51 | char* str = new char[30]; |
crmackey | 0:4d1ae7eb0070 | 52 | sprintf(str, "%.4g", batteryvoltage); |
crmackey | 0:4d1ae7eb0070 | 53 | m3pi.print(str,6); |
crmackey | 0:4d1ae7eb0070 | 54 | btbee.printf("Battery voltage is %f \n", batteryvoltage); |
crmackey | 0:4d1ae7eb0070 | 55 | wait(0.75); |
crmackey | 0:4d1ae7eb0070 | 56 | m3pi.cls(); |
crmackey | 0:4d1ae7eb0070 | 57 | |
crmackey | 0:4d1ae7eb0070 | 58 | if (batteryvoltage < 4.2) |
crmackey | 0:4d1ae7eb0070 | 59 | { |
crmackey | 0:4d1ae7eb0070 | 60 | char low[] = {'L','o','w',' ','b','a','t'}; |
crmackey | 0:4d1ae7eb0070 | 61 | m3pi.print(low,7); |
crmackey | 0:4d1ae7eb0070 | 62 | char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'}; |
crmackey | 0:4d1ae7eb0070 | 63 | m3pi.playtune(ExitSound,11); |
crmackey | 0:4d1ae7eb0070 | 64 | btbee.printf("Battery voltage is too low. Stopping program"); |
crmackey | 0:4d1ae7eb0070 | 65 | exit(1); |
crmackey | 0:4d1ae7eb0070 | 66 | } |
crmackey | 0:4d1ae7eb0070 | 67 | |
crmackey | 0:4d1ae7eb0070 | 68 | m3pi.locate(0,1); |
crmackey | 0:4d1ae7eb0070 | 69 | m3pi.printf("Line PID"); |
crmackey | 0:4d1ae7eb0070 | 70 | |
crmackey | 0:4d1ae7eb0070 | 71 | wait(2.0); |
crmackey | 6:75a6f6b50188 | 72 | |
crmackey | 0:4d1ae7eb0070 | 73 | btbee.printf("Now calibrating \n"); |
crmackey | 0:4d1ae7eb0070 | 74 | |
crmackey | 0:4d1ae7eb0070 | 75 | m3pi.sensor_auto_calibrate(); |
crmackey | 0:4d1ae7eb0070 | 76 | |
crmackey | 0:4d1ae7eb0070 | 77 | btbee.printf("Finished calibrating \n"); |
crmackey | 0:4d1ae7eb0070 | 78 | |
crmackey | 0:4d1ae7eb0070 | 79 | |
crmackey | 0:4d1ae7eb0070 | 80 | float right; |
crmackey | 0:4d1ae7eb0070 | 81 | float left; |
crmackey | 0:4d1ae7eb0070 | 82 | float current_pos_of_line = 0.0; |
crmackey | 0:4d1ae7eb0070 | 83 | float previous_pos_of_line = 0.0; |
crmackey | 0:4d1ae7eb0070 | 84 | float derivative,proportional,integral = 0; |
crmackey | 0:4d1ae7eb0070 | 85 | float power; |
crmackey | 0:4d1ae7eb0070 | 86 | float speed = MAX; |
crmackey | 0:4d1ae7eb0070 | 87 | int LapTest[5]; |
crmackey | 0:4d1ae7eb0070 | 88 | int s1, s2, s3, s4, s5; |
crmackey | 0:4d1ae7eb0070 | 89 | int counter = -1; |
crmackey | 0:4d1ae7eb0070 | 90 | int checkvar = 0; |
crmackey | 0:4d1ae7eb0070 | 91 | double Time1, Time2, Time3, Time4, Time5, TimeAve; |
crmackey | 3:ebe015c72bba | 92 | bool IR = 1; |
crmackey | 5:3f29267b18e3 | 93 | double timer; |
crmackey | 0:4d1ae7eb0070 | 94 | btbee.printf("\n"); |
crmackey | 0:4d1ae7eb0070 | 95 | btbee.printf("Now starting \n"); |
crmackey | 0:4d1ae7eb0070 | 96 | |
crmackey | 0:4d1ae7eb0070 | 97 | |
crmackey | 1:936e329e840a | 98 | |
crmackey | 0:4d1ae7eb0070 | 99 | |
crmackey | 1:936e329e840a | 100 | while (1) { |
crmackey | 1:936e329e840a | 101 | |
crmackey | 5:3f29267b18e3 | 102 | LoopTime.start(); |
crmackey | 5:3f29267b18e3 | 103 | |
crmackey | 3:ebe015c72bba | 104 | IR = m3pi_IN [0]; |
crmackey | 3:ebe015c72bba | 105 | |
crmackey | 3:ebe015c72bba | 106 | if(m3pi_IN [0] == 0 and IR == 0) { |
crmackey | 3:ebe015c72bba | 107 | while (m3pi_IN [0] == 0 and IR == 0) { |
crmackey | 4:0bcd7a924140 | 108 | m3pi.cls(); |
crmackey | 4:0bcd7a924140 | 109 | btbee.printf("Obstacle detected. Ending program"); |
crmackey | 4:0bcd7a924140 | 110 | m3pi.printf("ABORT"); |
crmackey | 2:1e947dcc40cb | 111 | m3pi.stop(); |
crmackey | 4:0bcd7a924140 | 112 | exit(1); |
crmackey | 2:1e947dcc40cb | 113 | } |
crmackey | 2:1e947dcc40cb | 114 | } |
crmackey | 4:0bcd7a924140 | 115 | |
crmackey | 2:1e947dcc40cb | 116 | |
crmackey | 2:1e947dcc40cb | 117 | // Get the position of the line. |
crmackey | 0:4d1ae7eb0070 | 118 | current_pos_of_line = m3pi.line_position(); |
crmackey | 0:4d1ae7eb0070 | 119 | proportional = current_pos_of_line; |
crmackey | 0:4d1ae7eb0070 | 120 | |
crmackey | 0:4d1ae7eb0070 | 121 | // Compute the derivative |
crmackey | 0:4d1ae7eb0070 | 122 | derivative = current_pos_of_line - previous_pos_of_line; |
crmackey | 0:4d1ae7eb0070 | 123 | |
crmackey | 0:4d1ae7eb0070 | 124 | // Compute the integral |
crmackey | 0:4d1ae7eb0070 | 125 | integral += proportional; |
crmackey | 0:4d1ae7eb0070 | 126 | |
crmackey | 0:4d1ae7eb0070 | 127 | // Remember the last position. |
crmackey | 0:4d1ae7eb0070 | 128 | previous_pos_of_line = current_pos_of_line; |
crmackey | 0:4d1ae7eb0070 | 129 | |
crmackey | 0:4d1ae7eb0070 | 130 | // Compute the power |
crmackey | 0:4d1ae7eb0070 | 131 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
crmackey | 0:4d1ae7eb0070 | 132 | |
crmackey | 0:4d1ae7eb0070 | 133 | // Compute new speeds |
crmackey | 0:4d1ae7eb0070 | 134 | right = speed+power; |
crmackey | 0:4d1ae7eb0070 | 135 | left = speed-power; |
crmackey | 0:4d1ae7eb0070 | 136 | |
crmackey | 0:4d1ae7eb0070 | 137 | // limit checks |
crmackey | 0:4d1ae7eb0070 | 138 | if (right < MIN) |
crmackey | 0:4d1ae7eb0070 | 139 | right = MIN; |
crmackey | 0:4d1ae7eb0070 | 140 | else if (right > MAX) |
crmackey | 0:4d1ae7eb0070 | 141 | right = MAX; |
crmackey | 0:4d1ae7eb0070 | 142 | |
crmackey | 0:4d1ae7eb0070 | 143 | if (left < MIN) |
crmackey | 0:4d1ae7eb0070 | 144 | left = MIN; |
crmackey | 0:4d1ae7eb0070 | 145 | else if (left > MAX) |
crmackey | 0:4d1ae7eb0070 | 146 | left = MAX; |
crmackey | 0:4d1ae7eb0070 | 147 | |
crmackey | 0:4d1ae7eb0070 | 148 | // set speed |
crmackey | 0:4d1ae7eb0070 | 149 | m3pi.left_motor(left); |
crmackey | 0:4d1ae7eb0070 | 150 | m3pi.right_motor(right); |
crmackey | 0:4d1ae7eb0070 | 151 | |
crmackey | 0:4d1ae7eb0070 | 152 | m3pi.calibrated_sensor(LapTest); |
crmackey | 0:4d1ae7eb0070 | 153 | |
crmackey | 0:4d1ae7eb0070 | 154 | s1 = LapTest[0]; |
crmackey | 0:4d1ae7eb0070 | 155 | s5 = LapTest[4]; |
crmackey | 0:4d1ae7eb0070 | 156 | |
crmackey | 8:e7b322f72d16 | 157 | if (s1 > 500 and s5 > 500) { |
crmackey | 8:e7b322f72d16 | 158 | m3pi.stop(); |
crmackey | 8:e7b322f72d16 | 159 | exit(1); |
crmackey | 8:e7b322f72d16 | 160 | } |
crmackey | 8:e7b322f72d16 | 161 | |
crmackey | 7:74c9a0fbf2b1 | 162 | // if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) { |
crmackey | 7:74c9a0fbf2b1 | 163 | // LapTimer.reset(); |
crmackey | 7:74c9a0fbf2b1 | 164 | // LapTimer.start(); |
crmackey | 7:74c9a0fbf2b1 | 165 | // } |
crmackey | 7:74c9a0fbf2b1 | 166 | // |
crmackey | 7:74c9a0fbf2b1 | 167 | // if (s1 < 500 and s5 < 500 and checkvar == 1) { |
crmackey | 7:74c9a0fbf2b1 | 168 | // counter = counter + 1; |
crmackey | 7:74c9a0fbf2b1 | 169 | // m3pi.cls(); |
crmackey | 7:74c9a0fbf2b1 | 170 | // m3pi.printf("%d", counter); |
crmackey | 7:74c9a0fbf2b1 | 171 | // checkvar = 0; |
crmackey | 7:74c9a0fbf2b1 | 172 | // switch (counter) { |
crmackey | 7:74c9a0fbf2b1 | 173 | // case 1: { |
crmackey | 7:74c9a0fbf2b1 | 174 | // Time1 = LapTimer.read(); |
crmackey | 7:74c9a0fbf2b1 | 175 | // LapTimer.reset(); |
crmackey | 7:74c9a0fbf2b1 | 176 | // LapTimer.start(); |
crmackey | 7:74c9a0fbf2b1 | 177 | // btbee.printf("Lap 1: %f \n", Time1); |
crmackey | 7:74c9a0fbf2b1 | 178 | // break; |
crmackey | 7:74c9a0fbf2b1 | 179 | // } |
crmackey | 7:74c9a0fbf2b1 | 180 | // case 2: { |
crmackey | 7:74c9a0fbf2b1 | 181 | // Time2 = LapTimer.read(); |
crmackey | 7:74c9a0fbf2b1 | 182 | // LapTimer.reset(); |
crmackey | 7:74c9a0fbf2b1 | 183 | // LapTimer.start(); |
crmackey | 7:74c9a0fbf2b1 | 184 | // btbee.printf("Lap 2: %f \n", Time2); |
crmackey | 7:74c9a0fbf2b1 | 185 | // break; |
crmackey | 7:74c9a0fbf2b1 | 186 | // } |
crmackey | 7:74c9a0fbf2b1 | 187 | // case 3: { |
crmackey | 7:74c9a0fbf2b1 | 188 | // Time3 = LapTimer.read(); |
crmackey | 7:74c9a0fbf2b1 | 189 | // LapTimer.reset(); |
crmackey | 7:74c9a0fbf2b1 | 190 | // LapTimer.start(); |
crmackey | 7:74c9a0fbf2b1 | 191 | // btbee.printf("Lap 3: %f \n", Time3); |
crmackey | 7:74c9a0fbf2b1 | 192 | // break; |
crmackey | 7:74c9a0fbf2b1 | 193 | // } |
crmackey | 7:74c9a0fbf2b1 | 194 | // case 4: { |
crmackey | 7:74c9a0fbf2b1 | 195 | // Time4 = LapTimer.read(); |
crmackey | 7:74c9a0fbf2b1 | 196 | // LapTimer.reset(); |
crmackey | 7:74c9a0fbf2b1 | 197 | // LapTimer.start(); |
crmackey | 7:74c9a0fbf2b1 | 198 | // btbee.printf("Lap 4: %f \n", Time4); |
crmackey | 7:74c9a0fbf2b1 | 199 | // break; |
crmackey | 7:74c9a0fbf2b1 | 200 | // } |
crmackey | 7:74c9a0fbf2b1 | 201 | // case 5: { |
crmackey | 7:74c9a0fbf2b1 | 202 | // Time5 = LapTimer.read(); |
crmackey | 7:74c9a0fbf2b1 | 203 | // LapTimer.reset(); |
crmackey | 7:74c9a0fbf2b1 | 204 | // LapTimer.start(); |
crmackey | 7:74c9a0fbf2b1 | 205 | // btbee.printf("Lap 5: %f \n", Time5); |
crmackey | 7:74c9a0fbf2b1 | 206 | // break; |
crmackey | 7:74c9a0fbf2b1 | 207 | // } |
crmackey | 7:74c9a0fbf2b1 | 208 | // } |
crmackey | 7:74c9a0fbf2b1 | 209 | // |
crmackey | 7:74c9a0fbf2b1 | 210 | // |
crmackey | 7:74c9a0fbf2b1 | 211 | // } |
crmackey | 7:74c9a0fbf2b1 | 212 | // |
crmackey | 7:74c9a0fbf2b1 | 213 | //if (counter == 5) { |
crmackey | 7:74c9a0fbf2b1 | 214 | // m3pi.stop(); |
crmackey | 7:74c9a0fbf2b1 | 215 | // m3pi.cls(); |
crmackey | 7:74c9a0fbf2b1 | 216 | // m3pi.locate(0,0); |
crmackey | 7:74c9a0fbf2b1 | 217 | // m3pi.printf("Ave time"); |
crmackey | 7:74c9a0fbf2b1 | 218 | // TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0; |
crmackey | 7:74c9a0fbf2b1 | 219 | // m3pi.locate(0,1); |
crmackey | 7:74c9a0fbf2b1 | 220 | // m3pi.printf("%f", TimeAve); |
crmackey | 7:74c9a0fbf2b1 | 221 | // btbee.printf("Average time per lap: %f \n", TimeAve); |
crmackey | 7:74c9a0fbf2b1 | 222 | // btbee.printf("\n"); |
crmackey | 7:74c9a0fbf2b1 | 223 | // btbee.printf("Enter P value, I value, and D value separately \n"); |
crmackey | 7:74c9a0fbf2b1 | 224 | // |
crmackey | 7:74c9a0fbf2b1 | 225 | // while(1) { |
crmackey | 7:74c9a0fbf2b1 | 226 | // |
crmackey | 7:74c9a0fbf2b1 | 227 | // |
crmackey | 7:74c9a0fbf2b1 | 228 | // if (btbee.readable()) { |
crmackey | 7:74c9a0fbf2b1 | 229 | // |
crmackey | 7:74c9a0fbf2b1 | 230 | // btbee.printf("Enter P valure: \n"); |
crmackey | 7:74c9a0fbf2b1 | 231 | // |
crmackey | 7:74c9a0fbf2b1 | 232 | // |
crmackey | 7:74c9a0fbf2b1 | 233 | // btbee.read_all(arr_read, 30, &chars_read); |
crmackey | 7:74c9a0fbf2b1 | 234 | // P_TERM = atof(arr_read); |
crmackey | 7:74c9a0fbf2b1 | 235 | // |
crmackey | 7:74c9a0fbf2b1 | 236 | // btbee.printf("P term is %f \n", P_TERM); |
crmackey | 7:74c9a0fbf2b1 | 237 | // |
crmackey | 7:74c9a0fbf2b1 | 238 | // wait(1.0); |
crmackey | 7:74c9a0fbf2b1 | 239 | // |
crmackey | 7:74c9a0fbf2b1 | 240 | // break; |
crmackey | 7:74c9a0fbf2b1 | 241 | // |
crmackey | 7:74c9a0fbf2b1 | 242 | // } |
crmackey | 7:74c9a0fbf2b1 | 243 | // |
crmackey | 7:74c9a0fbf2b1 | 244 | // } |
crmackey | 7:74c9a0fbf2b1 | 245 | // |
crmackey | 7:74c9a0fbf2b1 | 246 | // while(1) { |
crmackey | 7:74c9a0fbf2b1 | 247 | // |
crmackey | 7:74c9a0fbf2b1 | 248 | // if(btbee.readable()) { |
crmackey | 7:74c9a0fbf2b1 | 249 | // |
crmackey | 7:74c9a0fbf2b1 | 250 | // btbee.printf("Enter I value: \n"); |
crmackey | 7:74c9a0fbf2b1 | 251 | // |
crmackey | 7:74c9a0fbf2b1 | 252 | // btbee.read_all(arr_read, 30, &chars_read); |
crmackey | 7:74c9a0fbf2b1 | 253 | // I_TERM = atof(arr_read); |
crmackey | 7:74c9a0fbf2b1 | 254 | // |
crmackey | 7:74c9a0fbf2b1 | 255 | // btbee.printf("I term is %f \n", I_TERM); |
crmackey | 7:74c9a0fbf2b1 | 256 | // |
crmackey | 7:74c9a0fbf2b1 | 257 | // wait(1.0); |
crmackey | 7:74c9a0fbf2b1 | 258 | // |
crmackey | 7:74c9a0fbf2b1 | 259 | // break; |
crmackey | 7:74c9a0fbf2b1 | 260 | // |
crmackey | 7:74c9a0fbf2b1 | 261 | // } |
crmackey | 7:74c9a0fbf2b1 | 262 | // |
crmackey | 7:74c9a0fbf2b1 | 263 | // } |
crmackey | 7:74c9a0fbf2b1 | 264 | // |
crmackey | 7:74c9a0fbf2b1 | 265 | // while(1) { |
crmackey | 7:74c9a0fbf2b1 | 266 | // |
crmackey | 7:74c9a0fbf2b1 | 267 | // if(btbee.readable()) { |
crmackey | 7:74c9a0fbf2b1 | 268 | // |
crmackey | 7:74c9a0fbf2b1 | 269 | // btbee.printf("Enter D value: \n"); |
crmackey | 7:74c9a0fbf2b1 | 270 | // |
crmackey | 7:74c9a0fbf2b1 | 271 | // btbee.read_all(arr_read, 30, &chars_read); |
crmackey | 7:74c9a0fbf2b1 | 272 | // D_TERM = atof(arr_read); |
crmackey | 7:74c9a0fbf2b1 | 273 | // |
crmackey | 7:74c9a0fbf2b1 | 274 | // btbee.printf("D term is %f \n", D_TERM); |
crmackey | 7:74c9a0fbf2b1 | 275 | // |
crmackey | 7:74c9a0fbf2b1 | 276 | // wait(1); |
crmackey | 7:74c9a0fbf2b1 | 277 | // |
crmackey | 7:74c9a0fbf2b1 | 278 | // btbee.printf("Starting again with new parameters"); |
crmackey | 7:74c9a0fbf2b1 | 279 | // |
crmackey | 7:74c9a0fbf2b1 | 280 | // counter = -1; |
crmackey | 7:74c9a0fbf2b1 | 281 | // |
crmackey | 7:74c9a0fbf2b1 | 282 | // |
crmackey | 7:74c9a0fbf2b1 | 283 | // break; |
crmackey | 7:74c9a0fbf2b1 | 284 | // |
crmackey | 7:74c9a0fbf2b1 | 285 | // } |
crmackey | 7:74c9a0fbf2b1 | 286 | // |
crmackey | 7:74c9a0fbf2b1 | 287 | // } |
crmackey | 7:74c9a0fbf2b1 | 288 | // |
crmackey | 7:74c9a0fbf2b1 | 289 | // } |
crmackey | 6:75a6f6b50188 | 290 | |
crmackey | 6:75a6f6b50188 | 291 | |
crmackey | 5:3f29267b18e3 | 292 | |
crmackey | 6:75a6f6b50188 | 293 | |
crmackey | 6:75a6f6b50188 | 294 | //if(timer < 0.005) { |
crmackey | 6:75a6f6b50188 | 295 | // wait(0.005 - timer); |
crmackey | 6:75a6f6b50188 | 296 | // } |
crmackey | 6:75a6f6b50188 | 297 | // timer = 0; |
crmackey | 6:75a6f6b50188 | 298 | // LoopTime.reset(); |
crmackey | 0:4d1ae7eb0070 | 299 | |
crmackey | 0:4d1ae7eb0070 | 300 | } |
crmackey | 1:936e329e840a | 301 | |
crmackey | 2:1e947dcc40cb | 302 | |
crmackey | 2:1e947dcc40cb | 303 | |
crmackey | 6:75a6f6b50188 | 304 | } |