EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@28:9976a94efa83, 2014-12-08 (annotated)

Committer:
alecguertin
Date:
Mon Dec 08 10:52:42 2014 +0000
Revision:
28:9976a94efa83
Parent:
27:44a4a0abc8ee
Child:
29:459ff10d2a07
end of sunday

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