EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@19:47759cf4f9b9, 2014-12-01 (annotated)

Committer:
alecguertin
Date:
Mon Dec 01 20:16:23 2014 +0000
Revision:
19:47759cf4f9b9
Parent:
18:eab7b0e89398
Child:
20:76718145b403
added logic for turning/driving

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsaristo 1:7e0243c27ecb 1 /**
lsaristo 1:7e0243c27ecb 2 * @file driver.c
lsaristo 1:7e0243c27ecb 3 * @brief Basic driver program for our robot's controller logic.
lsaristo 1:7e0243c27ecb 4 *
lsaristo 1:7e0243c27ecb 5 * Maybe add lots of stuff here or maybe split it off into
lsaristo 1:7e0243c27ecb 6 * multiple subfiles?
lsaristo 1:7e0243c27ecb 7 *
alecguertin 19:47759cf4f9b9 8 * @author John Wilkey - aw hells no! you ain't takin' credit for all this!
lsaristo 1:7e0243c27ecb 9 */
lsaristo 9:3a0433c391cb 10 #include "main.h"
lsaristo 10:94b068b2ce1d 11 #include "control.h"
John Wilkey 5:01882c3de2dc 12
John Wilkey 5:01882c3de2dc 13 /**
John Wilkey 5:01882c3de2dc 14 * These are global data Used externally in all other files
John Wilkey 5:01882c3de2dc 15 */
lsaristo 7:6e5cc24e1ce7 16 m3pi pi;
lsaristo 12:1aa6b8a74136 17 Timer timer;
lsaristo 9:3a0433c391cb 18
lsaristo 9:3a0433c391cb 19 //
lsaristo 9:3a0433c391cb 20 // Digital inputs to the mBed
lsaristo 10:94b068b2ce1d 21 DigitalIn start_button(p21);
lsaristo 9:3a0433c391cb 22
lsaristo 9:3a0433c391cb 23 //
lsaristo 9:3a0433c391cb 24 // Digital outputs from the mBed. Note that by default these are
lsaristo 9:3a0433c391cb 25 // used to drive the 8 LED's on the top board.
lsaristo 7:6e5cc24e1ce7 26 DigitalOut pin15(p15);
lsaristo 7:6e5cc24e1ce7 27 DigitalOut pin16(p16);
lsaristo 7:6e5cc24e1ce7 28 DigitalOut pin17(p17);
lsaristo 7:6e5cc24e1ce7 29 DigitalOut pin18(p18);
lsaristo 7:6e5cc24e1ce7 30 DigitalOut pin19(p19);
lsaristo 7:6e5cc24e1ce7 31 DigitalOut pin20(p20);
lsaristo 7:6e5cc24e1ce7 32
lsaristo 9:3a0433c391cb 33 //
lsaristo 9:3a0433c391cb 34 // mBed onboard LEDs
lsaristo 9:3a0433c391cb 35 DigitalOut oled_1(LED1);
lsaristo 9:3a0433c391cb 36 DigitalOut oled_2(LED2);
lsaristo 9:3a0433c391cb 37 DigitalOut oled_3(LED3);
lsaristo 9:3a0433c391cb 38 DigitalOut oled_4(LED4);
lsaristo 9:3a0433c391cb 39
alecguertin 15:14d4e7021125 40 /* Local File System */
alecguertin 15:14d4e7021125 41 LocalFileSystem local("local");
lsaristo 9:3a0433c391cb 42
alecguertin 17:c72c092fcdf7 43 /* Boolean for drawing/moving. */
alecguertin 17:c72c092fcdf7 44 int draw;
alecguertin 17:c72c092fcdf7 45
John Wilkey 5:01882c3de2dc 46 /**
John Wilkey 5:01882c3de2dc 47 * @brief Entry point. Main loop.
John Wilkey 5:01882c3de2dc 48 */
John Wilkey 5:01882c3de2dc 49 int main()
John Wilkey 5:01882c3de2dc 50 {
alecguertin 17:c72c092fcdf7 51 FILE *ps_file;
alecguertin 17:c72c092fcdf7 52 int instbuflen = 250;
alecguertin 17:c72c092fcdf7 53 char instbuf[instbuflen];
alecguertin 17:c72c092fcdf7 54
lsaristo 9:3a0433c391cb 55 //
lsaristo 9:3a0433c391cb 56 // Basic setup information
lsaristo 10:94b068b2ce1d 57 start_button.mode(PullUp);
lsaristo 12:1aa6b8a74136 58
lsaristo 13:070846d87d4a 59 //
lsaristo 13:070846d87d4a 60 // Drawing environment calibration.
lsaristo 12:1aa6b8a74136 61 pi.sensor_auto_calibrate();
alecguertin 15:14d4e7021125 62 //pi.backward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 63 float pos = 0;
alecguertin 15:14d4e7021125 64 float over_thresh = 0.05;
alecguertin 15:14d4e7021125 65 float correction = 0.2*DRIVE_SPEED;
alecguertin 19:47759cf4f9b9 66 float cal_time;
alecguertin 19:47759cf4f9b9 67
alecguertin 15:14d4e7021125 68 wait(1);
alecguertin 15:14d4e7021125 69
alecguertin 15:14d4e7021125 70 do {
alecguertin 15:14d4e7021125 71 pos = pi.line_position();
alecguertin 15:14d4e7021125 72 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 73 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 74 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 75 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 76 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 77 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 78 } else {
alecguertin 15:14d4e7021125 79 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 80 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 81 }
alecguertin 15:14d4e7021125 82 /*
alecguertin 15:14d4e7021125 83 pi.cls();
alecguertin 15:14d4e7021125 84 pi.locate(0,0);
alecguertin 15:14d4e7021125 85 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 86 */
alecguertin 17:c72c092fcdf7 87 } while (pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 88 pi.stop();
alecguertin 15:14d4e7021125 89 wait(1);
alecguertin 15:14d4e7021125 90 if (pos != -1) {
alecguertin 15:14d4e7021125 91 timer.start();
alecguertin 15:14d4e7021125 92 pi.forward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 93 } else {
alecguertin 15:14d4e7021125 94 pi.cls();
alecguertin 15:14d4e7021125 95 pi.locate(0,0);
alecguertin 15:14d4e7021125 96 pi.printf("LP: %f",pos);
alecguertin 15:14d4e7021125 97 return 1;
alecguertin 15:14d4e7021125 98 }
alecguertin 15:14d4e7021125 99
alecguertin 15:14d4e7021125 100 wait(1);
lsaristo 12:1aa6b8a74136 101
lsaristo 12:1aa6b8a74136 102 do {
lsaristo 12:1aa6b8a74136 103 pos = pi.line_position();
lsaristo 13:070846d87d4a 104 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 105 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 106 pi.left_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 107 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 108 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 109 pi.right_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 110 } else {
alecguertin 15:14d4e7021125 111 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 112 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 113 }
alecguertin 15:14d4e7021125 114 pi.cls();
alecguertin 15:14d4e7021125 115 pi.locate(0,0);
alecguertin 15:14d4e7021125 116 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 117 } while(pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 118 pi.stop();
alecguertin 15:14d4e7021125 119 if(pos != -1) {
alecguertin 15:14d4e7021125 120 oled_1 = 1;
alecguertin 15:14d4e7021125 121 timer.stop();
alecguertin 15:14d4e7021125 122 pi.printf("T: %d", timer.read_ms());
alecguertin 15:14d4e7021125 123 } else {
alecguertin 15:14d4e7021125 124 pi.cls();
alecguertin 15:14d4e7021125 125 pi.locate(0,0);
alecguertin 15:14d4e7021125 126 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 127 return 1;
alecguertin 15:14d4e7021125 128 }
alecguertin 15:14d4e7021125 129 pi.right(TURN_SPEED);
alecguertin 15:14d4e7021125 130 wait(0.5);
alecguertin 15:14d4e7021125 131 pi.stop();
alecguertin 15:14d4e7021125 132 do {
alecguertin 15:14d4e7021125 133 pos = pi.line_position();
alecguertin 15:14d4e7021125 134 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 135 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 136 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 137 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 138 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 139 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 140 } else {
alecguertin 15:14d4e7021125 141 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 142 pi.left_motor(-DRIVE_SPEED);
lsaristo 13:070846d87d4a 143 }
lsaristo 12:1aa6b8a74136 144 pi.cls();
lsaristo 12:1aa6b8a74136 145 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 146 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 147 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 148 pi.stop();
alecguertin 15:14d4e7021125 149 if(pos != -1) {
alecguertin 15:14d4e7021125 150 oled_1 = 1;
alecguertin 15:14d4e7021125 151 timer.stop();
alecguertin 19:47759cf4f9b9 152 cal_time = timer.read();
alecguertin 15:14d4e7021125 153 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 154 } else {
lsaristo 12:1aa6b8a74136 155 pi.cls();
lsaristo 12:1aa6b8a74136 156 pi.locate(0,0);
alecguertin 15:14d4e7021125 157 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 158 return 1;
lsaristo 12:1aa6b8a74136 159 }
John Wilkey 6:00b7198f0b51 160
lsaristo 12:1aa6b8a74136 161 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 162 do {
lsaristo 12:1aa6b8a74136 163 pos = pi.line_position();
lsaristo 13:070846d87d4a 164 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 165 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 166 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 167 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 168 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 169 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 170 } else {
lsaristo 13:070846d87d4a 171 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 172 }
lsaristo 12:1aa6b8a74136 173 pi.cls();
lsaristo 12:1aa6b8a74136 174 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 175 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 176 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 177 if(pos == 1) {
lsaristo 12:1aa6b8a74136 178 oled_1 = 1;
lsaristo 12:1aa6b8a74136 179 timer.stop();
alecguertin 19:47759cf4f9b9 180 pi.stop();
lsaristo 12:1aa6b8a74136 181 } else {
lsaristo 12:1aa6b8a74136 182 pi.stop();
lsaristo 12:1aa6b8a74136 183 pi.cls();
lsaristo 12:1aa6b8a74136 184 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 185 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 186 while(1);
lsaristo 12:1aa6b8a74136 187 }
lsaristo 14:41fa8b95a9ab 188 // If we got here, calibration is complete.
lsaristo 12:1aa6b8a74136 189
John Wilkey 6:00b7198f0b51 190 //
lsaristo 9:3a0433c391cb 191 // Main program loop.
lsaristo 12:1aa6b8a74136 192 // robot_loop();
alecguertin 17:c72c092fcdf7 193 size_t bytes_read = 0;
alecguertin 19:47759cf4f9b9 194 int err, x, y, last_x, last_y, delta_x, delta_y, delta_a;
alecguertin 19:47759cf4f9b9 195 int dim_x, dim_y;
alecguertin 17:c72c092fcdf7 196 int offset = 0;
alecguertin 17:c72c092fcdf7 197 char *cur, *next;
alecguertin 19:47759cf4f9b9 198 float angle;
alecguertin 19:47759cf4f9b9 199 double dist, theta;
alecguertin 18:eab7b0e89398 200 angle = 0;
alecguertin 18:eab7b0e89398 201 theta = 0;
alecguertin 19:47759cf4f9b9 202 last_x = 0;
alecguertin 19:47759cf4f9b9 203 last_y = 0;
alecguertin 15:14d4e7021125 204 ps_file = fopen("/local/test.ps", "r");
alecguertin 17:c72c092fcdf7 205 /* PS parsing loop. */
alecguertin 19:47759cf4f9b9 206 memset(instbuf, 0, instbuflen);
alecguertin 19:47759cf4f9b9 207 bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file);
alecguertin 19:47759cf4f9b9 208 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 209 if (err != 2) {
alecguertin 19:47759cf4f9b9 210 return 1;
alecguertin 19:47759cf4f9b9 211 }
alecguertin 19:47759cf4f9b9 212 cur = strchr(instbuf, '\n');
alecguertin 17:c72c092fcdf7 213 while (1) {
alecguertin 17:c72c092fcdf7 214 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 215 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 17:c72c092fcdf7 216 if (bytes_read == 0) {
alecguertin 17:c72c092fcdf7 217 break;
alecguertin 17:c72c092fcdf7 218 }
alecguertin 17:c72c092fcdf7 219 cur = instbuf;
alecguertin 17:c72c092fcdf7 220 while (cur[0] != '\0') {
alecguertin 17:c72c092fcdf7 221 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 19:47759cf4f9b9 222 if (err == 0) {
alecguertin 19:47759cf4f9b9 223 return 1;
alecguertin 19:47759cf4f9b9 224 }
alecguertin 18:eab7b0e89398 225 delta_x = x-last_x;
alecguertin 18:eab7b0e89398 226 delta_y = y-last_y;
alecguertin 19:47759cf4f9b9 227
alecguertin 19:47759cf4f9b9 228 /* Compute turn angle and turn. */
alecguertin 19:47759cf4f9b9 229 theta = tan((double) delta_x/delta_y);
alecguertin 18:eab7b0e89398 230 theta *= 57.2957795;
alecguertin 18:eab7b0e89398 231 delta_a = theta-angle;
alecguertin 19:47759cf4f9b9 232 if (delta_a > 0) {
alecguertin 19:47759cf4f9b9 233 pi.left(TURN_SPEED);
alecguertin 19:47759cf4f9b9 234 } else {
alecguertin 19:47759cf4f9b9 235 pi.right(TURN_SPEED);
alecguertin 19:47759cf4f9b9 236 }
alecguertin 19:47759cf4f9b9 237 wait(0.5*(delta_a/360));
alecguertin 19:47759cf4f9b9 238
alecguertin 19:47759cf4f9b9 239 /* Put pen into position. */
alecguertin 19:47759cf4f9b9 240 if (draw) {
alecguertin 19:47759cf4f9b9 241 oled_1 = 1;
alecguertin 19:47759cf4f9b9 242 } else {
alecguertin 19:47759cf4f9b9 243 oled_1 = 0;
alecguertin 19:47759cf4f9b9 244 }
alecguertin 19:47759cf4f9b9 245 /* Compute drive time and move forward. */
alecguertin 19:47759cf4f9b9 246 dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
alecguertin 19:47759cf4f9b9 247 if (dist < 0) {
alecguertin 19:47759cf4f9b9 248 dist *= -1;
alecguertin 19:47759cf4f9b9 249 }
alecguertin 19:47759cf4f9b9 250 pi.forward(DRIVE_SPEED);
alecguertin 19:47759cf4f9b9 251 wait(cal_time*(dist/dim_x));
alecguertin 19:47759cf4f9b9 252
alecguertin 17:c72c092fcdf7 253 last_x = x;
alecguertin 17:c72c092fcdf7 254 last_y = y;
alecguertin 17:c72c092fcdf7 255 next = strchr(cur, '\n');
alecguertin 17:c72c092fcdf7 256 if (next == NULL) {
alecguertin 17:c72c092fcdf7 257 break;
alecguertin 17:c72c092fcdf7 258 }
alecguertin 17:c72c092fcdf7 259 cur = next+1;
alecguertin 17:c72c092fcdf7 260 }
alecguertin 17:c72c092fcdf7 261 offset = instbuf+instbuflen-cur;
alecguertin 17:c72c092fcdf7 262 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 263 }
lsaristo 9:3a0433c391cb 264 //
lsaristo 9:3a0433c391cb 265 // We should never reach this point!
alecguertin 18:eab7b0e89398 266 //
alecguertin 18:eab7b0e89398 267 // Yes, we should...
alecguertin 15:14d4e7021125 268 return 0;
John Wilkey 5:01882c3de2dc 269 }
lsaristo 8:12d780f7443e 270
alecguertin 17:c72c092fcdf7 271 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 272 {
alecguertin 17:c72c092fcdf7 273 int matches;
alecguertin 17:c72c092fcdf7 274 char *srch;
alecguertin 17:c72c092fcdf7 275 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 276 if (matches != 2) {
alecguertin 17:c72c092fcdf7 277 return 0;
alecguertin 17:c72c092fcdf7 278 }
alecguertin 17:c72c092fcdf7 279 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 280 srch++;
alecguertin 17:c72c092fcdf7 281 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 282 srch++;
alecguertin 17:c72c092fcdf7 283 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 284 *draw = 0;
alecguertin 17:c72c092fcdf7 285 } else if (srch[0] == 'd') {
alecguertin 17:c72c092fcdf7 286 *draw = 1;
alecguertin 17:c72c092fcdf7 287 } else {
alecguertin 17:c72c092fcdf7 288 return 0;
alecguertin 17:c72c092fcdf7 289 }
alecguertin 17:c72c092fcdf7 290 return 1;
alecguertin 17:c72c092fcdf7 291 }
alecguertin 17:c72c092fcdf7 292
lsaristo 9:3a0433c391cb 293 int forward(int amt)
lsaristo 8:12d780f7443e 294 {
lsaristo 12:1aa6b8a74136 295 Timer t;
lsaristo 12:1aa6b8a74136 296 t.start();
lsaristo 10:94b068b2ce1d 297 oled_2 = 1;
lsaristo 9:3a0433c391cb 298 pi.locate(0,0);
lsaristo 9:3a0433c391cb 299 pi.printf("Fwd %d", amt);
lsaristo 9:3a0433c391cb 300 pi.forward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 301 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 302 t.stop();
lsaristo 10:94b068b2ce1d 303 oled_2 = 0;
lsaristo 12:1aa6b8a74136 304 pi.stop();
lsaristo 8:12d780f7443e 305 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 306 }
lsaristo 8:12d780f7443e 307
lsaristo 9:3a0433c391cb 308 int backward(int amt)
lsaristo 8:12d780f7443e 309 {
lsaristo 12:1aa6b8a74136 310 Timer t;
lsaristo 10:94b068b2ce1d 311 oled_3 = 1;
lsaristo 9:3a0433c391cb 312 pi.locate(0,0);
lsaristo 9:3a0433c391cb 313 pi.printf("Back %d", amt);
lsaristo 9:3a0433c391cb 314 pi.backward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 315 t.start();
lsaristo 12:1aa6b8a74136 316 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 317 t.stop();
lsaristo 10:94b068b2ce1d 318 oled_3 = 0;
lsaristo 12:1aa6b8a74136 319 pi.stop();
lsaristo 8:12d780f7443e 320 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 321 }
lsaristo 8:12d780f7443e 322
lsaristo 8:12d780f7443e 323 int right(float deg)
lsaristo 8:12d780f7443e 324 {
lsaristo 12:1aa6b8a74136 325 Timer t;
lsaristo 10:94b068b2ce1d 326 oled_4 = 1;
lsaristo 9:3a0433c391cb 327 pi.locate(0,0);
lsaristo 9:3a0433c391cb 328 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 329 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 330 t.start();
lsaristo 12:1aa6b8a74136 331 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 332 t.stop();
lsaristo 10:94b068b2ce1d 333 oled_4 = 0;
lsaristo 12:1aa6b8a74136 334 pi.stop();
lsaristo 8:12d780f7443e 335 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 336 }
lsaristo 8:12d780f7443e 337
lsaristo 8:12d780f7443e 338 int left(float deg)
lsaristo 8:12d780f7443e 339 {
lsaristo 12:1aa6b8a74136 340 Timer t;
lsaristo 10:94b068b2ce1d 341 oled_4 = 1;
lsaristo 10:94b068b2ce1d 342 oled_2 = 1;
lsaristo 9:3a0433c391cb 343 pi.locate(0,0);
lsaristo 9:3a0433c391cb 344 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 345 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 346 t.start();
lsaristo 12:1aa6b8a74136 347 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 348 t.stop();
lsaristo 10:94b068b2ce1d 349 oled_4 = 0;
lsaristo 10:94b068b2ce1d 350 oled_2 = 0;
lsaristo 12:1aa6b8a74136 351 pi.stop();
lsaristo 8:12d780f7443e 352 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 353 }
lsaristo 10:94b068b2ce1d 354
lsaristo 10:94b068b2ce1d 355 void pen_down()
lsaristo 10:94b068b2ce1d 356 {
lsaristo 10:94b068b2ce1d 357 oled_1 = 1;
lsaristo 10:94b068b2ce1d 358 }
lsaristo 10:94b068b2ce1d 359
lsaristo 10:94b068b2ce1d 360 void pen_up()
lsaristo 10:94b068b2ce1d 361 {
lsaristo 10:94b068b2ce1d 362 oled_1 = 0;
lsaristo 9:3a0433c391cb 363 }