EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@46:a3b9c0fe8f36, 2014-12-19 (annotated)

Committer:
lsaristo
Date:
Fri Dec 19 03:47:18 2014 +0000
Revision:
46:a3b9c0fe8f36
Parent:
45:b0f1d06aa6df 
Changed postscript file name to control.ps instead.

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