EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@43:a0c9549a6f8a, 2014-12-18 (annotated)

Committer:
alecguertin
Date:
Thu Dec 18 04:22:24 2014 +0000
Revision:
43:a0c9549a6f8a
Parent:
42:727612987d77
Child:
44:fd755ef0f862
added states for initialization

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