EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@44:fd755ef0f862, 2014-12-18 (annotated)

Committer:
alecguertin
Date:
Thu Dec 18 05:20:51 2014 +0000
Revision:
44:fd755ef0f862
Parent:
43:a0c9549a6f8a
Child:
45:b0f1d06aa6df
trying find_line

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