EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@42:727612987d77, 2014-12-18 (annotated)

Committer:
alecguertin
Date:
Thu Dec 18 03:53:42 2014 +0000
Revision:
42:727612987d77
Parent:
41:1b6d8664d7a7
Child:
43:a0c9549a6f8a
added finished state and fix for reading instructions

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