EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@37:1d51cf101b03, 2014-12-12 (annotated)

Committer:
alecguertin
Date:
Fri Dec 12 20:53:45 2014 +0000
Revision:
37:1d51cf101b03
Parent:
35:a1c14c6d9282
Child:
39:cc8691700d2a
working!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsaristo 1:7e0243c27ecb 1 /**
lsaristo 1:7e0243c27ecb 2 * @file driver.c
lsaristo 29:459ff10d2a07 3 * @brief Primary control logic for robot.
lsaristo 1:7e0243c27ecb 4 *
lsaristo 29:459ff10d2a07 5 * @author John Wilkey
lsaristo 29:459ff10d2a07 6 * @author Alec Guertin
lsaristo 29:459ff10d2a07 7 * @author Chester Chu
lsaristo 1:7e0243c27ecb 8 */
lsaristo 9:3a0433c391cb 9 #include "main.h"
John Wilkey 5:01882c3de2dc 10
lsaristo 29:459ff10d2a07 11 m3pi pi; /**< m3pi Object for all control operations. */
lsaristo 29:459ff10d2a07 12 Timer timer; /**< Timer used to wait in timerWait. */
lsaristo 29:459ff10d2a07 13 DigitalIn start_button(p21); /**< Pi start button input on mBed pin 32 */
lsaristo 29:459ff10d2a07 14 int draw; /**< Boolean for drawing/moving. */
lsaristo 9:3a0433c391cb 15
lsaristo 29:459ff10d2a07 16 /* 4 mBed onboard LEDs */
lsaristo 29:459ff10d2a07 17 DigitalOut oled_1(LED1), oled_2(LED2), oled_3(LED3), oled_4(LED4);
lsaristo 7:6e5cc24e1ce7 18
lsaristo 29:459ff10d2a07 19 /* 8 LEDs driven from mBed digital outs. */
lsaristo 29:459ff10d2a07 20 DigitalOut pin13(p13), pin14(p14), pin15(p15), pin16(p16),
lsaristo 29:459ff10d2a07 21 pin17(p17), pin18(p18), pin19(p19), pin20(p20);
lsaristo 9:3a0433c391cb 22
alecguertin 15:14d4e7021125 23 /* Local File System */
alecguertin 15:14d4e7021125 24 LocalFileSystem local("local");
lsaristo 9:3a0433c391cb 25
lsaristo 29:459ff10d2a07 26 /** @brief Entry point. Main loop. */
John Wilkey 5:01882c3de2dc 27 int main()
John Wilkey 5:01882c3de2dc 28 {
lsaristo 29:459ff10d2a07 29 FILE *ps_file;
lsaristo 29:459ff10d2a07 30 float cal_time;
lsaristo 29:459ff10d2a07 31 float pos = 0;
lsaristo 29:459ff10d2a07 32 float over_thresh = 0.05;
lsaristo 29:459ff10d2a07 33 float correction = 0.2*DRIVE_SPEED;
lsaristo 29:459ff10d2a07 34 int instbuflen = 250;
lsaristo 29:459ff10d2a07 35 char instbuf[instbuflen];
lsaristo 29:459ff10d2a07 36
lsaristo 32:8b589710632b 37 // ---------------------------------------------------------
lsaristo 32:8b589710632b 38 // First calibrate robot and figure out space dimensions.
lsaristo 21:0c80a5d89ea3 39 pi.cls();
lsaristo 21:0c80a5d89ea3 40 pi.locate(0,1);
lsaristo 21:0c80a5d89ea3 41 pi.printf("%f mV", pi.battery());
lsaristo 12:1aa6b8a74136 42 pi.sensor_auto_calibrate();
lsaristo 34:3066686d5152 43 timerWait(.2);
alecguertin 26:a74edc4c6acb 44
lsaristo 21:0c80a5d89ea3 45 do {
lsaristo 21:0c80a5d89ea3 46 pos = pi.line_position();
lsaristo 34:3066686d5152 47 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 48 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 49 pi.left_motor(CAL_SPEED-correction);
lsaristo 21:0c80a5d89ea3 50 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 51 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 52 pi.right_motor(CAL_SPEED-correction);
lsaristo 21:0c80a5d89ea3 53 } else {
lsaristo 21:0c80a5d89ea3 54 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 55 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 56 }
lsaristo 21:0c80a5d89ea3 57 pi.cls();
lsaristo 21:0c80a5d89ea3 58 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 59 pi.printf("Pos: %f", pos);
lsaristo 21:0c80a5d89ea3 60 } while(pos != -1 && pos > -0.3);
lsaristo 21:0c80a5d89ea3 61 pi.stop();
lsaristo 21:0c80a5d89ea3 62 if(pos != -1) {
lsaristo 21:0c80a5d89ea3 63 timer.stop();
lsaristo 21:0c80a5d89ea3 64 } else {
lsaristo 34:3066686d5152 65 oled_1 = 1;
lsaristo 34:3066686d5152 66 while(1);
lsaristo 21:0c80a5d89ea3 67 }
chstrchu 31:1e950ee04481 68 left(180);
alecguertin 22:46b9d9b2e35c 69 Timer caltimer;
alecguertin 22:46b9d9b2e35c 70 caltimer.start();
lsaristo 12:1aa6b8a74136 71 do {
lsaristo 12:1aa6b8a74136 72 pos = pi.line_position();
lsaristo 13:070846d87d4a 73 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 74 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 75 pi.left_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 76 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 77 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 78 pi.right_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 79 } else {
lsaristo 21:0c80a5d89ea3 80 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 81 pi.left_motor(CAL_SPEED);
alecguertin 15:14d4e7021125 82 }
alecguertin 15:14d4e7021125 83 pi.cls();
alecguertin 15:14d4e7021125 84 pi.locate(0,0);
alecguertin 15:14d4e7021125 85 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 86 } while(pos != -1 && pos <= 0.3);
alecguertin 22:46b9d9b2e35c 87 caltimer.stop();
lsaristo 34:3066686d5152 88 cal_time = caltimer.read_ms()*2;
alecguertin 15:14d4e7021125 89 pi.stop();
alecguertin 15:14d4e7021125 90 if(pos != -1) {
alecguertin 15:14d4e7021125 91 timer.stop();
alecguertin 15:14d4e7021125 92 } else {
lsaristo 34:3066686d5152 93 oled_1 = 1;
lsaristo 34:3066686d5152 94 while(1);
alecguertin 15:14d4e7021125 95 }
alecguertin 37:1d51cf101b03 96 right(183);
lsaristo 32:8b589710632b 97 timerWait(.2);
alecguertin 37:1d51cf101b03 98 /*int wiggle_count = 0;
lsaristo 32:8b589710632b 99
lsaristo 35:a1c14c6d9282 100 for( ; fabs(pos = pi.line_position()) > CLOSE_ENOUGH
lsaristo 35:a1c14c6d9282 101 && wiggle_count < WIGGLE_MAX; wiggle_count++) {
lsaristo 35:a1c14c6d9282 102 pi.right((pos < 0 ? -.6*.4*CAL_SPEED : .6*.4*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 103 pi.cls();
alecguertin 22:46b9d9b2e35c 104 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 105 pi.printf("O: %f", pos);
alecguertin 37:1d51cf101b03 106 }*/
alecguertin 37:1d51cf101b03 107 pi.stop();
lsaristo 32:8b589710632b 108 timerWait(0.2);
lsaristo 34:3066686d5152 109 backward(400);
alecguertin 37:1d51cf101b03 110 //while(1);
lsaristo 34:3066686d5152 111
lsaristo 32:8b589710632b 112 // ---------------------------------------------------------
lsaristo 32:8b589710632b 113 // DONE calibrating. Begin reading in coordinate
lsaristo 32:8b589710632b 114 // file and run robot.
alecguertin 17:c72c092fcdf7 115 size_t bytes_read = 0;
alecguertin 22:46b9d9b2e35c 116 int err, x, y, last_x, last_y, delta_x, delta_y;
alecguertin 22:46b9d9b2e35c 117 float delta_a;
alecguertin 19:47759cf4f9b9 118 int dim_x, dim_y;
alecguertin 17:c72c092fcdf7 119 int offset = 0;
alecguertin 17:c72c092fcdf7 120 char *cur, *next;
alecguertin 19:47759cf4f9b9 121 float angle;
alecguertin 19:47759cf4f9b9 122 double dist, theta;
alecguertin 18:eab7b0e89398 123 angle = 0;
alecguertin 18:eab7b0e89398 124 theta = 0;
alecguertin 19:47759cf4f9b9 125 last_x = 0;
alecguertin 19:47759cf4f9b9 126 last_y = 0;
alecguertin 15:14d4e7021125 127 ps_file = fopen("/local/test.ps", "r");
chstrchu 20:76718145b403 128 if (ps_file == NULL) {
chstrchu 20:76718145b403 129 return 1;
chstrchu 20:76718145b403 130 }
alecguertin 25:2c7717684d09 131
alecguertin 17:c72c092fcdf7 132 /* PS parsing loop. */
alecguertin 19:47759cf4f9b9 133 memset(instbuf, 0, instbuflen);
chstrchu 20:76718145b403 134 bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
alecguertin 22:46b9d9b2e35c 135 if (bytes_read == 0) {
alecguertin 22:46b9d9b2e35c 136 return 1;
alecguertin 22:46b9d9b2e35c 137 }
alecguertin 19:47759cf4f9b9 138 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 139 if (err != 2) {
chstrchu 20:76718145b403 140 pi.cls();
chstrchu 20:76718145b403 141 pi.locate(0,0);
chstrchu 20:76718145b403 142 pi.printf("sscanf1");
lsaristo 34:3066686d5152 143 while(1);
alecguertin 19:47759cf4f9b9 144 }
alecguertin 19:47759cf4f9b9 145 cur = strchr(instbuf, '\n');
chstrchu 20:76718145b403 146 cur++;
alecguertin 22:46b9d9b2e35c 147 offset = instbuf+instbuflen-cur-1;
chstrchu 20:76718145b403 148 memcpy(instbuf, cur, offset);
lsaristo 32:8b589710632b 149
lsaristo 32:8b589710632b 150
lsaristo 32:8b589710632b 151 // ---------------------------------------------------------
lsaristo 32:8b589710632b 152 // File open and buffer setup. Begin reading instructions and
lsaristo 32:8b589710632b 153 // moving robot. Refill buffer after each instruction.
alecguertin 17:c72c092fcdf7 154 while (1) {
alecguertin 17:c72c092fcdf7 155 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 156 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 28:9976a94efa83 157
alecguertin 27:44a4a0abc8ee 158 if (instbuf[0] == '\0') {
alecguertin 28:9976a94efa83 159 pi.cls();
alecguertin 28:9976a94efa83 160 pi.locate(0,0);
alecguertin 28:9976a94efa83 161 pi.printf("%s", instbuf+1);
alecguertin 37:1d51cf101b03 162 oled_3 = 0;
alecguertin 28:9976a94efa83 163 while(1);
alecguertin 27:44a4a0abc8ee 164 }
alecguertin 28:9976a94efa83 165
alecguertin 17:c72c092fcdf7 166 cur = instbuf;
alecguertin 22:46b9d9b2e35c 167 err = retrieve_inst(instbuf, &x, &y, &draw);
lsaristo 32:8b589710632b 168
alecguertin 22:46b9d9b2e35c 169 if (err == 0) {
chstrchu 20:76718145b403 170 pi.cls();
chstrchu 20:76718145b403 171 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 172 pi.printf("noinst");
alecguertin 22:46b9d9b2e35c 173 return 1;
alecguertin 22:46b9d9b2e35c 174 }
lsaristo 32:8b589710632b 175
alecguertin 22:46b9d9b2e35c 176 delta_x = x-last_x;
alecguertin 22:46b9d9b2e35c 177 delta_y = y-last_y;
lsaristo 34:3066686d5152 178 if (delta_y == 0) {
lsaristo 34:3066686d5152 179 if (delta_x < 0) {
lsaristo 34:3066686d5152 180 theta = 180;
lsaristo 34:3066686d5152 181 } else {
lsaristo 34:3066686d5152 182 theta = 0;
lsaristo 34:3066686d5152 183 }
lsaristo 34:3066686d5152 184 } else if (delta_x == 0) {
lsaristo 34:3066686d5152 185 if (delta_y < 0) {
lsaristo 34:3066686d5152 186 theta = -90;
lsaristo 34:3066686d5152 187 } else {
lsaristo 34:3066686d5152 188 theta = 90;
lsaristo 34:3066686d5152 189 }
lsaristo 34:3066686d5152 190 } else {
lsaristo 34:3066686d5152 191 /* Compute turn angle and turn. */
lsaristo 34:3066686d5152 192 theta = atan(((double) delta_y)/((double) delta_x));
lsaristo 34:3066686d5152 193 theta *= 57.29;
lsaristo 34:3066686d5152 194 }
alecguertin 27:44a4a0abc8ee 195 if (delta_x < 0 && delta_y > 0) {
alecguertin 27:44a4a0abc8ee 196 theta += 180;
alecguertin 27:44a4a0abc8ee 197 }
alecguertin 22:46b9d9b2e35c 198 delta_a = theta-angle;
lsaristo 32:8b589710632b 199
alecguertin 23:e4616259a7f0 200 if (delta_x < 0 && delta_y < 0) {
alecguertin 23:e4616259a7f0 201 delta_a += 180;
alecguertin 23:e4616259a7f0 202 }
lsaristo 32:8b589710632b 203
alecguertin 28:9976a94efa83 204 if (delta_a > 180) {
alecguertin 28:9976a94efa83 205 delta_a -= 360;
lsaristo 32:8b589710632b 206 } else if (delta_a < -180) {
lsaristo 30:3211e2962441 207 delta_a = 360 + delta_a;
lsaristo 30:3211e2962441 208 }
alecguertin 25:2c7717684d09 209 angle += delta_a;
alecguertin 28:9976a94efa83 210 if (angle >= 360) {
alecguertin 28:9976a94efa83 211 angle -= 360;
alecguertin 28:9976a94efa83 212 }
chstrchu 31:1e950ee04481 213 pi.cls();
chstrchu 31:1e950ee04481 214 pi.locate(0,0);
chstrchu 31:1e950ee04481 215 pi.printf("a:%f", delta_a);
alecguertin 37:1d51cf101b03 216 oled_3 = 0;
alecguertin 25:2c7717684d09 217 left(delta_a);
alecguertin 19:47759cf4f9b9 218
alecguertin 22:46b9d9b2e35c 219 /* Put pen into position. */
alecguertin 22:46b9d9b2e35c 220 if (draw) {
alecguertin 28:9976a94efa83 221 oled_3 = 1;
alecguertin 22:46b9d9b2e35c 222 } else {
alecguertin 28:9976a94efa83 223 oled_3 = 0;
alecguertin 22:46b9d9b2e35c 224 }
alecguertin 22:46b9d9b2e35c 225
alecguertin 22:46b9d9b2e35c 226 /* Compute drive time and move forward. */
alecguertin 22:46b9d9b2e35c 227 dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
alecguertin 22:46b9d9b2e35c 228 if (dist < 0) {
alecguertin 22:46b9d9b2e35c 229 dist *= -1;
alecguertin 22:46b9d9b2e35c 230 }
chstrchu 31:1e950ee04481 231 pi.cls();
chstrchu 31:1e950ee04481 232 pi.locate(0,0);
lsaristo 34:3066686d5152 233 pi.printf("d:%f", 0.55*cal_time*(dist/(double)dim_x));
lsaristo 34:3066686d5152 234 forward(0.55*cal_time*(dist/(double)dim_x));
alecguertin 22:46b9d9b2e35c 235 last_x = x;
alecguertin 22:46b9d9b2e35c 236 last_y = y;
alecguertin 22:46b9d9b2e35c 237 next = strchr(cur, '\n');
alecguertin 22:46b9d9b2e35c 238 if (next == NULL) {
chstrchu 20:76718145b403 239 pi.cls();
chstrchu 20:76718145b403 240 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 241 pi.printf("nonext");
alecguertin 22:46b9d9b2e35c 242 break;
alecguertin 17:c72c092fcdf7 243 }
alecguertin 22:46b9d9b2e35c 244 cur = next+1;
alecguertin 22:46b9d9b2e35c 245 offset = instbuf+instbuflen-cur-1;
alecguertin 17:c72c092fcdf7 246 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 247 }
chstrchu 20:76718145b403 248 pi.cls();
chstrchu 20:76718145b403 249 pi.locate(0,0);
lsaristo 29:459ff10d2a07 250 pi.printf("Done");
lsaristo 34:3066686d5152 251 pi.stop();
lsaristo 34:3066686d5152 252 oled_3 = 0;
alecguertin 27:44a4a0abc8ee 253 while (1);
John Wilkey 5:01882c3de2dc 254 }
lsaristo 8:12d780f7443e 255
alecguertin 17:c72c092fcdf7 256 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 257 {
alecguertin 17:c72c092fcdf7 258 int matches;
alecguertin 17:c72c092fcdf7 259 char *srch;
alecguertin 17:c72c092fcdf7 260 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 261 if (matches != 2) {
chstrchu 20:76718145b403 262 pi.cls();
chstrchu 20:76718145b403 263 pi.locate(0,0);
chstrchu 20:76718145b403 264 pi.printf("nomatch");
alecguertin 17:c72c092fcdf7 265 return 0;
alecguertin 17:c72c092fcdf7 266 }
alecguertin 17:c72c092fcdf7 267 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 268 srch++;
chstrchu 20:76718145b403 269 srch = strchr(srch, ' ');
alecguertin 17:c72c092fcdf7 270 srch++;
alecguertin 17:c72c092fcdf7 271 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 272 *draw = 0;
chstrchu 20:76718145b403 273 } else if (srch[0] == 'l') {
alecguertin 17:c72c092fcdf7 274 *draw = 1;
alecguertin 17:c72c092fcdf7 275 } else {
chstrchu 20:76718145b403 276 pi.cls();
chstrchu 20:76718145b403 277 pi.locate(0,0);
chstrchu 20:76718145b403 278 pi.printf("%.8s", srch);
alecguertin 17:c72c092fcdf7 279 return 0;
alecguertin 17:c72c092fcdf7 280 }
alecguertin 17:c72c092fcdf7 281 return 1;
alecguertin 17:c72c092fcdf7 282 }
alecguertin 17:c72c092fcdf7 283
lsaristo 29:459ff10d2a07 284 void forward(int amt)
lsaristo 8:12d780f7443e 285 {
lsaristo 12:1aa6b8a74136 286 Timer t;
lsaristo 34:3066686d5152 287 float ms = 0;
lsaristo 34:3066686d5152 288 float last_ms = 0;
lsaristo 9:3a0433c391cb 289 pi.locate(0,0);
lsaristo 9:3a0433c391cb 290 pi.printf("Fwd %d", amt);
lsaristo 30:3211e2962441 291 t.start();
lsaristo 34:3066686d5152 292 pi.left_motor(DRIVE_SPEED+.002);
lsaristo 21:0c80a5d89ea3 293 pi.right_motor(DRIVE_SPEED);
lsaristo 34:3066686d5152 294 ms = t.read_ms();
lsaristo 34:3066686d5152 295 while(ms < amt) {
alecguertin 37:1d51cf101b03 296 if (ms-last_ms > 500) {
lsaristo 34:3066686d5152 297 t.stop();
alecguertin 37:1d51cf101b03 298 right(1.5);
lsaristo 34:3066686d5152 299 last_ms = ms;
lsaristo 34:3066686d5152 300 t.start();
lsaristo 34:3066686d5152 301 pi.left_motor(DRIVE_SPEED+.002);
lsaristo 34:3066686d5152 302 pi.right_motor(DRIVE_SPEED);
lsaristo 34:3066686d5152 303 }
lsaristo 34:3066686d5152 304 ms = t.read_ms();
lsaristo 34:3066686d5152 305 }
lsaristo 12:1aa6b8a74136 306 pi.stop();
lsaristo 8:12d780f7443e 307 }
lsaristo 8:12d780f7443e 308
lsaristo 29:459ff10d2a07 309 void backward(int amt)
lsaristo 8:12d780f7443e 310 {
lsaristo 12:1aa6b8a74136 311 Timer t;
lsaristo 12:1aa6b8a74136 312 t.start();
alecguertin 22:46b9d9b2e35c 313 pi.backward(.5*DRIVE_SPEED);
chstrchu 20:76718145b403 314 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 315 pi.stop();
lsaristo 8:12d780f7443e 316 }
lsaristo 8:12d780f7443e 317
lsaristo 29:459ff10d2a07 318 void right(float deg)
lsaristo 8:12d780f7443e 319 {
alecguertin 37:1d51cf101b03 320 deg += 2;
lsaristo 24:b797563776fc 321 if(deg < 0) {
lsaristo 24:b797563776fc 322 return left(-1*deg);
lsaristo 24:b797563776fc 323 }
alecguertin 37:1d51cf101b03 324 deg = deg;
lsaristo 12:1aa6b8a74136 325 Timer t;
alecguertin 37:1d51cf101b03 326 float amt = ((((float)deg)/360)*TIME_FACT*1000);
lsaristo 12:1aa6b8a74136 327 t.start();
lsaristo 30:3211e2962441 328 pi.right(TURN_SPEED);
alecguertin 37:1d51cf101b03 329 while(t.read_us() < amt);
lsaristo 12:1aa6b8a74136 330 pi.stop();
lsaristo 8:12d780f7443e 331 }
lsaristo 8:12d780f7443e 332
lsaristo 21:0c80a5d89ea3 333 void timerWait(float seconds)
lsaristo 21:0c80a5d89ea3 334 {
lsaristo 21:0c80a5d89ea3 335 Timer t;
lsaristo 21:0c80a5d89ea3 336 t.start();
lsaristo 21:0c80a5d89ea3 337 while(t.read_us() < 1000000*seconds);
lsaristo 21:0c80a5d89ea3 338 }
lsaristo 21:0c80a5d89ea3 339
lsaristo 29:459ff10d2a07 340 void left(float deg)
lsaristo 8:12d780f7443e 341 {
lsaristo 24:b797563776fc 342 if(deg < 0) {
lsaristo 24:b797563776fc 343 return right(-1*deg);
lsaristo 24:b797563776fc 344 }
alecguertin 37:1d51cf101b03 345 float amt = (((float)deg)/360)*TIME_FACT*1000;
lsaristo 12:1aa6b8a74136 346 Timer t;
lsaristo 8:12d780f7443e 347 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 348 t.start();
alecguertin 37:1d51cf101b03 349 while(t.read_us() < amt);
lsaristo 12:1aa6b8a74136 350 pi.stop();
lsaristo 9:3a0433c391cb 351 }