EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@40:0199bad6c979, 2014-12-17 (annotated)

Committer:
alecguertin
Date:
Wed Dec 17 05:13:37 2014 +0000
Revision:
40:0199bad6c979
Parent:
39:cc8691700d2a
Child:
41:1b6d8664d7a7
hi john

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 {
alecguertin 39:cc8691700d2a 29 Timer caltimer;
alecguertin 39:cc8691700d2a 30 FILE *ps_file;
alecguertin 39:cc8691700d2a 31 double dist;
alecguertin 39:cc8691700d2a 32 float delta_a, angle, cal_time;
alecguertin 39:cc8691700d2a 33 size_t bytes_read;
alecguertin 39:cc8691700d2a 34 int corner, err, x, y, last_x, last_y, dim_x, dim_y, offset;
alecguertin 39:cc8691700d2a 35 char instbuf[INST_BUF_SIZE];
alecguertin 39:cc8691700d2a 36 char *cur, *next;
alecguertin 39:cc8691700d2a 37 angle = 0;
alecguertin 39:cc8691700d2a 38 last_x = 0;
alecguertin 39:cc8691700d2a 39 last_y = 0;
alecguertin 39:cc8691700d2a 40 bytes_read = 0;
alecguertin 39:cc8691700d2a 41 offset = 0;
alecguertin 39:cc8691700d2a 42 int init = 0;
alecguertin 26:a74edc4c6acb 43
alecguertin 39:cc8691700d2a 44 /* First calibrate robot and figure out space dimensions. */
alecguertin 39:cc8691700d2a 45 robot_printf(0, "%f mV", pi.battery());
alecguertin 39:cc8691700d2a 46
alecguertin 39:cc8691700d2a 47 /* Calibrate robot reflective sensors to line. */
alecguertin 39:cc8691700d2a 48 pi.sensor_auto_calibrate();
alecguertin 39:cc8691700d2a 49 timerWait(0.2);
alecguertin 39:cc8691700d2a 50
alecguertin 39:cc8691700d2a 51 /* Find left bottom right corner of frame. */
alecguertin 39:cc8691700d2a 52 corner = find_corner(1);
alecguertin 39:cc8691700d2a 53 if (!corner) {
alecguertin 39:cc8691700d2a 54 robot_printf(0, "no left\n");
alecguertin 39:cc8691700d2a 55 while (1);
lsaristo 21:0c80a5d89ea3 56 }
alecguertin 39:cc8691700d2a 57 left(HALF_TURN);
alecguertin 39:cc8691700d2a 58
alecguertin 39:cc8691700d2a 59 /*
alecguertin 39:cc8691700d2a 60 * Find bottom left corner of frame and
alecguertin 39:cc8691700d2a 61 * and measure length of frame side.
alecguertin 39:cc8691700d2a 62 */
alecguertin 22:46b9d9b2e35c 63 caltimer.start();
alecguertin 39:cc8691700d2a 64 corner = find_corner(0);
alecguertin 39:cc8691700d2a 65 if (!corner) {
alecguertin 39:cc8691700d2a 66 robot_printf(0, "no right\n");
alecguertin 39:cc8691700d2a 67 while (1);
alecguertin 39:cc8691700d2a 68 }
alecguertin 22:46b9d9b2e35c 69 caltimer.stop();
alecguertin 39:cc8691700d2a 70 cal_time = caltimer.read_ms();
alecguertin 39:cc8691700d2a 71
alecguertin 39:cc8691700d2a 72 /* Back up into corner (coordinate x=0,y=0). */
alecguertin 39:cc8691700d2a 73 right(HALF_TURN);
lsaristo 32:8b589710632b 74 timerWait(0.2);
lsaristo 34:3066686d5152 75 backward(400);
alecguertin 39:cc8691700d2a 76
alecguertin 39:cc8691700d2a 77 /* Open instruction file. */
alecguertin 15:14d4e7021125 78 ps_file = fopen("/local/test.ps", "r");
chstrchu 20:76718145b403 79 if (ps_file == NULL) {
chstrchu 20:76718145b403 80 return 1;
chstrchu 20:76718145b403 81 }
alecguertin 25:2c7717684d09 82
alecguertin 39:cc8691700d2a 83 /* Read beginning of file into buffer. */
alecguertin 39:cc8691700d2a 84 memset(instbuf, 0, INST_BUF_SIZE);
alecguertin 39:cc8691700d2a 85 bytes_read = fread(instbuf, sizeof(char), INST_BUF_SIZE-1, ps_file);
alecguertin 22:46b9d9b2e35c 86 if (bytes_read == 0) {
alecguertin 22:46b9d9b2e35c 87 return 1;
alecguertin 22:46b9d9b2e35c 88 }
alecguertin 39:cc8691700d2a 89
alecguertin 39:cc8691700d2a 90 /* Read first line of file - the dimensions of the frame. */
alecguertin 19:47759cf4f9b9 91 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 92 if (err != 2) {
alecguertin 39:cc8691700d2a 93 robot_printf(0, "sscanf1");
lsaristo 34:3066686d5152 94 while(1);
alecguertin 19:47759cf4f9b9 95 }
alecguertin 19:47759cf4f9b9 96 cur = strchr(instbuf, '\n');
chstrchu 20:76718145b403 97 cur++;
alecguertin 39:cc8691700d2a 98 offset = instbuf+INST_BUF_SIZE-cur-1;
chstrchu 20:76718145b403 99 memcpy(instbuf, cur, offset);
alecguertin 39:cc8691700d2a 100 /*
alecguertin 39:cc8691700d2a 101 * File open and buffer setup. Begin reading instructions and
alecguertin 39:cc8691700d2a 102 * moving robot. Refill buffer after each instruction.
alecguertin 39:cc8691700d2a 103 */
alecguertin 17:c72c092fcdf7 104 while (1) {
alecguertin 39:cc8691700d2a 105 memset(instbuf+offset, 0, INST_BUF_SIZE-offset);
alecguertin 39:cc8691700d2a 106 bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file);
alecguertin 27:44a4a0abc8ee 107 if (instbuf[0] == '\0') {
alecguertin 39:cc8691700d2a 108 robot_printf(0, "%s", instbuf+1);
alecguertin 40:0199bad6c979 109 pen_up();
alecguertin 28:9976a94efa83 110 while(1);
alecguertin 27:44a4a0abc8ee 111 }
alecguertin 28:9976a94efa83 112
alecguertin 17:c72c092fcdf7 113 cur = instbuf;
alecguertin 22:46b9d9b2e35c 114 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 22:46b9d9b2e35c 115 if (err == 0) {
alecguertin 39:cc8691700d2a 116 robot_printf(0, "noinst");
alecguertin 22:46b9d9b2e35c 117 return 1;
alecguertin 22:46b9d9b2e35c 118 }
lsaristo 32:8b589710632b 119
alecguertin 39:cc8691700d2a 120 if (0 && !draw && init) {
alecguertin 39:cc8691700d2a 121 float pos;
alecguertin 39:cc8691700d2a 122 right(angle+QUARTER_TURN);
alecguertin 39:cc8691700d2a 123 find_line();
alecguertin 39:cc8691700d2a 124 forward(100);
alecguertin 39:cc8691700d2a 125 pi.left(CAL_SPEED);
alecguertin 39:cc8691700d2a 126 pos = pi.line_position();
alecguertin 39:cc8691700d2a 127 while (pos > CORRECTION_THRESHOLD || pos < 0) {
alecguertin 39:cc8691700d2a 128 pos = pi.line_position();
lsaristo 34:3066686d5152 129 }
alecguertin 39:cc8691700d2a 130 pi.stop();
alecguertin 39:cc8691700d2a 131 find_corner(1);
alecguertin 39:cc8691700d2a 132 left(180);
alecguertin 39:cc8691700d2a 133 find_corner(0);
alecguertin 39:cc8691700d2a 134 right(HALF_TURN);
alecguertin 39:cc8691700d2a 135 timerWait(0.5);
alecguertin 39:cc8691700d2a 136 backward(400);
alecguertin 39:cc8691700d2a 137 angle = 0;
alecguertin 39:cc8691700d2a 138 last_x = 0;
alecguertin 39:cc8691700d2a 139 last_y = 0;
alecguertin 23:e4616259a7f0 140 }
lsaristo 32:8b589710632b 141
alecguertin 39:cc8691700d2a 142 init = 1;
alecguertin 39:cc8691700d2a 143 /* Compute turn angle and turn. */
alecguertin 39:cc8691700d2a 144 delta_a = compute_turn_angle(last_x, last_y, x, y, angle);
alecguertin 25:2c7717684d09 145 angle += delta_a;
alecguertin 39:cc8691700d2a 146 if (angle >= FULL_TURN) {
alecguertin 39:cc8691700d2a 147 angle -= FULL_TURN;
alecguertin 28:9976a94efa83 148 }
alecguertin 39:cc8691700d2a 149 robot_printf(0, "a:%f", delta_a);
alecguertin 25:2c7717684d09 150 left(delta_a);
alecguertin 19:47759cf4f9b9 151
alecguertin 40:0199bad6c979 152 dist = fabs(distance(last_x, last_y, x, y));
alecguertin 40:0199bad6c979 153 robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x));
alecguertin 40:0199bad6c979 154
alecguertin 22:46b9d9b2e35c 155 /* Put pen into position. */
alecguertin 22:46b9d9b2e35c 156 if (draw) {
alecguertin 40:0199bad6c979 157 pen_down();
alecguertin 22:46b9d9b2e35c 158 } else {
alecguertin 40:0199bad6c979 159 pen_up();
alecguertin 22:46b9d9b2e35c 160 }
alecguertin 22:46b9d9b2e35c 161
alecguertin 22:46b9d9b2e35c 162 /* Compute drive time and move forward. */
alecguertin 39:cc8691700d2a 163 forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
alecguertin 40:0199bad6c979 164 pen_up();
alecguertin 22:46b9d9b2e35c 165 last_x = x;
alecguertin 22:46b9d9b2e35c 166 last_y = y;
alecguertin 39:cc8691700d2a 167
alecguertin 39:cc8691700d2a 168 /* Seek to find next instruction. */
alecguertin 22:46b9d9b2e35c 169 next = strchr(cur, '\n');
alecguertin 22:46b9d9b2e35c 170 if (next == NULL) {
alecguertin 39:cc8691700d2a 171 robot_printf(0, "nonext");
alecguertin 22:46b9d9b2e35c 172 break;
alecguertin 17:c72c092fcdf7 173 }
alecguertin 22:46b9d9b2e35c 174 cur = next+1;
alecguertin 39:cc8691700d2a 175 offset = instbuf+INST_BUF_SIZE-cur-1;
alecguertin 17:c72c092fcdf7 176 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 177 }
alecguertin 39:cc8691700d2a 178 robot_printf(0, "Done");
lsaristo 34:3066686d5152 179 pi.stop();
alecguertin 40:0199bad6c979 180 pen_up();
alecguertin 27:44a4a0abc8ee 181 while (1);
John Wilkey 5:01882c3de2dc 182 }
lsaristo 8:12d780f7443e 183
alecguertin 39:cc8691700d2a 184 float compute_turn_angle(int last_x, int last_y, int x, int y, float angle)
alecguertin 39:cc8691700d2a 185 {
alecguertin 39:cc8691700d2a 186 int delta_x, delta_y;
alecguertin 39:cc8691700d2a 187 float theta, delta_a;
alecguertin 39:cc8691700d2a 188 delta_x = x-last_x;
alecguertin 39:cc8691700d2a 189 delta_y = y-last_y;
alecguertin 39:cc8691700d2a 190 if (delta_y == 0) {
alecguertin 39:cc8691700d2a 191 if (delta_x < 0) {
alecguertin 39:cc8691700d2a 192 theta = HALF_TURN;
alecguertin 39:cc8691700d2a 193 } else {
alecguertin 39:cc8691700d2a 194 theta = 0;
alecguertin 39:cc8691700d2a 195 }
alecguertin 39:cc8691700d2a 196 } else if (delta_x == 0) {
alecguertin 39:cc8691700d2a 197 if (delta_y < 0) {
alecguertin 39:cc8691700d2a 198 theta = -QUARTER_TURN;
alecguertin 39:cc8691700d2a 199 } else {
alecguertin 39:cc8691700d2a 200 theta = QUARTER_TURN;
alecguertin 39:cc8691700d2a 201 }
alecguertin 39:cc8691700d2a 202 } else {
alecguertin 39:cc8691700d2a 203 theta = atan(((double) delta_y)/((double) delta_x));
alecguertin 39:cc8691700d2a 204 theta *= RAD_TO_DEG;
alecguertin 39:cc8691700d2a 205 }
alecguertin 39:cc8691700d2a 206 if (delta_x < 0 && delta_y > 0) {
alecguertin 39:cc8691700d2a 207 theta += HALF_TURN;
alecguertin 39:cc8691700d2a 208 }
alecguertin 39:cc8691700d2a 209 delta_a = theta-angle;
alecguertin 39:cc8691700d2a 210
alecguertin 39:cc8691700d2a 211 if (delta_x < 0 && delta_y < 0) {
alecguertin 39:cc8691700d2a 212 delta_a += HALF_TURN;
alecguertin 39:cc8691700d2a 213 }
alecguertin 39:cc8691700d2a 214
alecguertin 39:cc8691700d2a 215 if (delta_a > HALF_TURN) {
alecguertin 39:cc8691700d2a 216 delta_a -= FULL_TURN;
alecguertin 39:cc8691700d2a 217 } else if (delta_a < -HALF_TURN) {
alecguertin 39:cc8691700d2a 218 delta_a = FULL_TURN + delta_a;
alecguertin 39:cc8691700d2a 219 }
alecguertin 39:cc8691700d2a 220 return delta_a;
alecguertin 39:cc8691700d2a 221 }
alecguertin 39:cc8691700d2a 222
alecguertin 39:cc8691700d2a 223 void robot_printf(int line, const char *format, ...)
alecguertin 39:cc8691700d2a 224 {
alecguertin 39:cc8691700d2a 225 char buf[18];
alecguertin 39:cc8691700d2a 226 va_list args;
alecguertin 39:cc8691700d2a 227 pi.cls();
alecguertin 39:cc8691700d2a 228 pi.locate(0,line);
alecguertin 39:cc8691700d2a 229 va_start(args, format);
alecguertin 39:cc8691700d2a 230 vsprintf(buf, format, args);
alecguertin 39:cc8691700d2a 231 pi.printf("%s", buf);
alecguertin 39:cc8691700d2a 232 va_end(args);
alecguertin 39:cc8691700d2a 233 }
alecguertin 39:cc8691700d2a 234
alecguertin 39:cc8691700d2a 235 float distance(int x1, int y1, int x2, int y2)
alecguertin 39:cc8691700d2a 236 {
alecguertin 39:cc8691700d2a 237 return sqrt(pow((double) (x2-x1),2) + pow((double) (y2-y1), 2));
alecguertin 39:cc8691700d2a 238 }
alecguertin 39:cc8691700d2a 239
alecguertin 39:cc8691700d2a 240 void find_line()
alecguertin 39:cc8691700d2a 241 {
alecguertin 39:cc8691700d2a 242 float pos, pos1;
alecguertin 39:cc8691700d2a 243 pi.forward(DRIVE_SPEED);
alecguertin 39:cc8691700d2a 244 pos1 = pi.line_position();
alecguertin 39:cc8691700d2a 245 pos = pos1;
alecguertin 39:cc8691700d2a 246 while (fabs(pos1-pos) < 0.1 || pos == -1) {
alecguertin 39:cc8691700d2a 247 pos = pi.line_position();
alecguertin 39:cc8691700d2a 248 robot_printf(0, "%f\n", pos);
alecguertin 39:cc8691700d2a 249 }
alecguertin 39:cc8691700d2a 250 robot_printf(0, "f:%f\n", pos);
alecguertin 39:cc8691700d2a 251 pi.stop();
alecguertin 39:cc8691700d2a 252 }
alecguertin 39:cc8691700d2a 253
alecguertin 39:cc8691700d2a 254 int find_corner(int left)
alecguertin 39:cc8691700d2a 255 {
alecguertin 39:cc8691700d2a 256 float pos;
alecguertin 39:cc8691700d2a 257 float threshold = CORNER_THRESHOLD;
alecguertin 39:cc8691700d2a 258 if (left) {
alecguertin 39:cc8691700d2a 259 threshold *= -1;
alecguertin 39:cc8691700d2a 260 }
alecguertin 39:cc8691700d2a 261 do {
alecguertin 39:cc8691700d2a 262 pos = pi.line_position();
alecguertin 39:cc8691700d2a 263 if(pos > CORRECTION_THRESHOLD) {
alecguertin 39:cc8691700d2a 264 pi.right_motor(CAL_SPEED);
alecguertin 39:cc8691700d2a 265 pi.left_motor(CAL_SPEED-CORRECTION_SPEED);
alecguertin 39:cc8691700d2a 266 } else if(pos < -CORRECTION_THRESHOLD) {
alecguertin 39:cc8691700d2a 267 pi.left_motor(CAL_SPEED);
alecguertin 39:cc8691700d2a 268 pi.right_motor(CAL_SPEED-CORRECTION_SPEED);
alecguertin 39:cc8691700d2a 269 } else {
alecguertin 39:cc8691700d2a 270 pi.right_motor(CAL_SPEED);
alecguertin 39:cc8691700d2a 271 pi.left_motor(CAL_SPEED);
alecguertin 39:cc8691700d2a 272 }
alecguertin 39:cc8691700d2a 273 robot_printf(0, "Pos: %f", pos);
alecguertin 39:cc8691700d2a 274 } while(pos != -1 && ((left && pos > threshold) || (!left && pos < threshold)));
alecguertin 39:cc8691700d2a 275 pi.stop();
alecguertin 39:cc8691700d2a 276 if(pos == -1) {
alecguertin 39:cc8691700d2a 277 oled_1 = 1;
alecguertin 39:cc8691700d2a 278 return 0;
alecguertin 39:cc8691700d2a 279 }
alecguertin 39:cc8691700d2a 280 return 1;
alecguertin 39:cc8691700d2a 281 }
alecguertin 39:cc8691700d2a 282
alecguertin 17:c72c092fcdf7 283 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 284 {
alecguertin 17:c72c092fcdf7 285 int matches;
alecguertin 17:c72c092fcdf7 286 char *srch;
alecguertin 17:c72c092fcdf7 287 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 288 if (matches != 2) {
alecguertin 39:cc8691700d2a 289 robot_printf(0, "nomatch");
alecguertin 17:c72c092fcdf7 290 return 0;
alecguertin 17:c72c092fcdf7 291 }
alecguertin 17:c72c092fcdf7 292 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 293 srch++;
chstrchu 20:76718145b403 294 srch = strchr(srch, ' ');
alecguertin 17:c72c092fcdf7 295 srch++;
alecguertin 17:c72c092fcdf7 296 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 297 *draw = 0;
chstrchu 20:76718145b403 298 } else if (srch[0] == 'l') {
alecguertin 17:c72c092fcdf7 299 *draw = 1;
alecguertin 17:c72c092fcdf7 300 } else {
alecguertin 39:cc8691700d2a 301 robot_printf(0, "%.8s", srch);
alecguertin 17:c72c092fcdf7 302 return 0;
alecguertin 17:c72c092fcdf7 303 }
alecguertin 17:c72c092fcdf7 304 return 1;
alecguertin 17:c72c092fcdf7 305 }
alecguertin 17:c72c092fcdf7 306
lsaristo 29:459ff10d2a07 307 void forward(int amt)
lsaristo 8:12d780f7443e 308 {
lsaristo 12:1aa6b8a74136 309 Timer t;
lsaristo 34:3066686d5152 310 float ms = 0;
lsaristo 34:3066686d5152 311 float last_ms = 0;
alecguertin 39:cc8691700d2a 312 robot_printf(0, "Fwd %d", amt);
lsaristo 30:3211e2962441 313 t.start();
lsaristo 34:3066686d5152 314 pi.left_motor(DRIVE_SPEED+.002);
lsaristo 21:0c80a5d89ea3 315 pi.right_motor(DRIVE_SPEED);
lsaristo 34:3066686d5152 316 ms = t.read_ms();
lsaristo 34:3066686d5152 317 while(ms < amt) {
alecguertin 37:1d51cf101b03 318 if (ms-last_ms > 500) {
lsaristo 34:3066686d5152 319 t.stop();
alecguertin 37:1d51cf101b03 320 right(1.5);
lsaristo 34:3066686d5152 321 last_ms = ms;
lsaristo 34:3066686d5152 322 t.start();
lsaristo 34:3066686d5152 323 pi.left_motor(DRIVE_SPEED+.002);
lsaristo 34:3066686d5152 324 pi.right_motor(DRIVE_SPEED);
lsaristo 34:3066686d5152 325 }
lsaristo 34:3066686d5152 326 ms = t.read_ms();
lsaristo 34:3066686d5152 327 }
lsaristo 12:1aa6b8a74136 328 pi.stop();
lsaristo 8:12d780f7443e 329 }
lsaristo 8:12d780f7443e 330
lsaristo 29:459ff10d2a07 331 void backward(int amt)
lsaristo 8:12d780f7443e 332 {
lsaristo 12:1aa6b8a74136 333 Timer t;
lsaristo 12:1aa6b8a74136 334 t.start();
alecguertin 22:46b9d9b2e35c 335 pi.backward(.5*DRIVE_SPEED);
chstrchu 20:76718145b403 336 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 337 pi.stop();
lsaristo 8:12d780f7443e 338 }
lsaristo 8:12d780f7443e 339
alecguertin 39:cc8691700d2a 340 void left(float deg)
alecguertin 39:cc8691700d2a 341 {
alecguertin 39:cc8691700d2a 342 Timer t;
alecguertin 39:cc8691700d2a 343 deg += DEGREE_CORRECTION;
alecguertin 39:cc8691700d2a 344 if(deg < 0) {
alecguertin 39:cc8691700d2a 345 return right(-1*deg);
alecguertin 39:cc8691700d2a 346 }
alecguertin 39:cc8691700d2a 347 float amt = (((float)deg)/FULL_TURN)*TIME_FACT*1000;
alecguertin 39:cc8691700d2a 348 pi.left(TURN_SPEED);
alecguertin 39:cc8691700d2a 349 t.start();
alecguertin 39:cc8691700d2a 350 while(t.read_us() < amt);
alecguertin 39:cc8691700d2a 351 pi.stop();
alecguertin 39:cc8691700d2a 352 }
alecguertin 39:cc8691700d2a 353
lsaristo 29:459ff10d2a07 354 void right(float deg)
lsaristo 8:12d780f7443e 355 {
alecguertin 39:cc8691700d2a 356 Timer t;
alecguertin 39:cc8691700d2a 357 deg += DEGREE_CORRECTION;
lsaristo 24:b797563776fc 358 if(deg < 0) {
lsaristo 24:b797563776fc 359 return left(-1*deg);
lsaristo 24:b797563776fc 360 }
alecguertin 39:cc8691700d2a 361 float amt = ((((float)deg)/FULL_TURN)*TIME_FACT*1000);
lsaristo 12:1aa6b8a74136 362 t.start();
lsaristo 30:3211e2962441 363 pi.right(TURN_SPEED);
alecguertin 37:1d51cf101b03 364 while(t.read_us() < amt);
lsaristo 12:1aa6b8a74136 365 pi.stop();
lsaristo 8:12d780f7443e 366 }
lsaristo 8:12d780f7443e 367
lsaristo 21:0c80a5d89ea3 368 void timerWait(float seconds)
lsaristo 21:0c80a5d89ea3 369 {
lsaristo 21:0c80a5d89ea3 370 Timer t;
lsaristo 21:0c80a5d89ea3 371 t.start();
lsaristo 21:0c80a5d89ea3 372 while(t.read_us() < 1000000*seconds);
alecguertin 40:0199bad6c979 373 }
alecguertin 40:0199bad6c979 374
alecguertin 40:0199bad6c979 375 void pen_down()
alecguertin 40:0199bad6c979 376 {
alecguertin 40:0199bad6c979 377 oled_3 = 1;
alecguertin 40:0199bad6c979 378 }
alecguertin 40:0199bad6c979 379
alecguertin 40:0199bad6c979 380 void pen_up()
alecguertin 40:0199bad6c979 381 {
alecguertin 40:0199bad6c979 382 oled_3 = 0;
alecguertin 40:0199bad6c979 383 }