EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@21:0c80a5d89ea3, 2014-12-06 (annotated)

Committer:
lsaristo
Date:
Sat Dec 06 22:04:53 2014 +0000
Revision:
21:0c80a5d89ea3
Parent:
20:76718145b403
Child:
22:46b9d9b2e35c
Worked on calibrating robot movements. Accurate enough for now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsaristo 1:7e0243c27ecb 1 /**
lsaristo 1:7e0243c27ecb 2 * @file driver.c
lsaristo 1:7e0243c27ecb 3 * @brief Basic driver program for our robot's controller logic.
lsaristo 1:7e0243c27ecb 4 *
lsaristo 1:7e0243c27ecb 5 * Maybe add lots of stuff here or maybe split it off into
lsaristo 1:7e0243c27ecb 6 * multiple subfiles?
lsaristo 1:7e0243c27ecb 7 *
alecguertin 19:47759cf4f9b9 8 * @author John Wilkey - aw hells no! you ain't takin' credit for all this!
lsaristo 1:7e0243c27ecb 9 */
lsaristo 9:3a0433c391cb 10 #include "main.h"
lsaristo 10:94b068b2ce1d 11 #include "control.h"
John Wilkey 5:01882c3de2dc 12
John Wilkey 5:01882c3de2dc 13 /**
John Wilkey 5:01882c3de2dc 14 * These are global data Used externally in all other files
John Wilkey 5:01882c3de2dc 15 */
lsaristo 7:6e5cc24e1ce7 16 m3pi pi;
lsaristo 12:1aa6b8a74136 17 Timer timer;
lsaristo 9:3a0433c391cb 18
lsaristo 9:3a0433c391cb 19 //
lsaristo 9:3a0433c391cb 20 // Digital inputs to the mBed
lsaristo 10:94b068b2ce1d 21 DigitalIn start_button(p21);
lsaristo 9:3a0433c391cb 22
lsaristo 9:3a0433c391cb 23 //
lsaristo 9:3a0433c391cb 24 // Digital outputs from the mBed. Note that by default these are
lsaristo 9:3a0433c391cb 25 // used to drive the 8 LED's on the top board.
lsaristo 7:6e5cc24e1ce7 26 DigitalOut pin15(p15);
lsaristo 7:6e5cc24e1ce7 27 DigitalOut pin16(p16);
lsaristo 7:6e5cc24e1ce7 28 DigitalOut pin17(p17);
lsaristo 7:6e5cc24e1ce7 29 DigitalOut pin18(p18);
lsaristo 7:6e5cc24e1ce7 30 DigitalOut pin19(p19);
lsaristo 7:6e5cc24e1ce7 31 DigitalOut pin20(p20);
lsaristo 7:6e5cc24e1ce7 32
lsaristo 9:3a0433c391cb 33 //
lsaristo 9:3a0433c391cb 34 // mBed onboard LEDs
lsaristo 9:3a0433c391cb 35 DigitalOut oled_1(LED1);
lsaristo 9:3a0433c391cb 36 DigitalOut oled_2(LED2);
lsaristo 9:3a0433c391cb 37 DigitalOut oled_3(LED3);
lsaristo 9:3a0433c391cb 38 DigitalOut oled_4(LED4);
lsaristo 9:3a0433c391cb 39
alecguertin 15:14d4e7021125 40 /* Local File System */
alecguertin 15:14d4e7021125 41 LocalFileSystem local("local");
lsaristo 9:3a0433c391cb 42
alecguertin 17:c72c092fcdf7 43 /* Boolean for drawing/moving. */
alecguertin 17:c72c092fcdf7 44 int draw;
alecguertin 17:c72c092fcdf7 45
John Wilkey 5:01882c3de2dc 46 /**
John Wilkey 5:01882c3de2dc 47 * @brief Entry point. Main loop.
John Wilkey 5:01882c3de2dc 48 */
John Wilkey 5:01882c3de2dc 49 int main()
John Wilkey 5:01882c3de2dc 50 {
alecguertin 17:c72c092fcdf7 51 FILE *ps_file;
alecguertin 17:c72c092fcdf7 52 int instbuflen = 250;
alecguertin 17:c72c092fcdf7 53 char instbuf[instbuflen];
alecguertin 17:c72c092fcdf7 54
lsaristo 9:3a0433c391cb 55 //
lsaristo 9:3a0433c391cb 56 // Basic setup information
lsaristo 10:94b068b2ce1d 57 start_button.mode(PullUp);
lsaristo 12:1aa6b8a74136 58
lsaristo 21:0c80a5d89ea3 59 start:
lsaristo 21:0c80a5d89ea3 60 pi.cls();
lsaristo 21:0c80a5d89ea3 61 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 62 pi.printf("PiCO");
lsaristo 21:0c80a5d89ea3 63 pi.locate(0,1);
lsaristo 21:0c80a5d89ea3 64 pi.printf("%f mV", pi.battery());
lsaristo 21:0c80a5d89ea3 65 wait(.5);
lsaristo 21:0c80a5d89ea3 66 /* while(start_button) {
lsaristo 21:0c80a5d89ea3 67 oled_2 = 1;
lsaristo 21:0c80a5d89ea3 68 wait(.5);
lsaristo 21:0c80a5d89ea3 69 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 70 pi.printf("Ready");
lsaristo 21:0c80a5d89ea3 71 oled_2 = 0;
lsaristo 21:0c80a5d89ea3 72 } /**/
lsaristo 21:0c80a5d89ea3 73 pi.cls();
lsaristo 21:0c80a5d89ea3 74 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 75 // pi.printf("GO!");
lsaristo 21:0c80a5d89ea3 76 wait(.5);
lsaristo 21:0c80a5d89ea3 77
lsaristo 21:0c80a5d89ea3 78 /*
lsaristo 21:0c80a5d89ea3 79 while(1) {
lsaristo 21:0c80a5d89ea3 80 if(!start_button) {
lsaristo 21:0c80a5d89ea3 81 pi.stop();
lsaristo 21:0c80a5d89ea3 82 goto start;
lsaristo 21:0c80a5d89ea3 83 } else {
lsaristo 21:0c80a5d89ea3 84 goto calibrate;
lsaristo 21:0c80a5d89ea3 85 }
lsaristo 21:0c80a5d89ea3 86 }
lsaristo 21:0c80a5d89ea3 87 /**/
lsaristo 21:0c80a5d89ea3 88
lsaristo 13:070846d87d4a 89 //
lsaristo 13:070846d87d4a 90 // Drawing environment calibration.
lsaristo 21:0c80a5d89ea3 91 int cal_count = 1;
lsaristo 12:1aa6b8a74136 92 pi.sensor_auto_calibrate();
alecguertin 15:14d4e7021125 93 //pi.backward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 94 float pos = 0;
alecguertin 15:14d4e7021125 95 float over_thresh = 0.05;
alecguertin 15:14d4e7021125 96 float correction = 0.2*DRIVE_SPEED;
alecguertin 19:47759cf4f9b9 97 float cal_time;
alecguertin 15:14d4e7021125 98 wait(1);
lsaristo 21:0c80a5d89ea3 99 /*
alecguertin 15:14d4e7021125 100 do {
alecguertin 15:14d4e7021125 101 pos = pi.line_position();
alecguertin 15:14d4e7021125 102 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 103 pi.left_motor(-CAL_SPEED);
lsaristo 21:0c80a5d89ea3 104 pi.right_motor(-CAL_SPEED+correction);
alecguertin 15:14d4e7021125 105 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 106 pi.right_motor(-CAL_SPEED);
lsaristo 21:0c80a5d89ea3 107 pi.left_motor(-CAL_SPEED+correction);
alecguertin 15:14d4e7021125 108 } else {
lsaristo 21:0c80a5d89ea3 109 pi.backward(CAL_SPEED);
alecguertin 15:14d4e7021125 110 }
alecguertin 15:14d4e7021125 111 pi.cls();
alecguertin 15:14d4e7021125 112 pi.locate(0,0);
alecguertin 15:14d4e7021125 113 pi.printf("P: %f", pos);
alecguertin 17:c72c092fcdf7 114 } while (pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 115 pi.stop();
alecguertin 15:14d4e7021125 116 wait(1);
lsaristo 21:0c80a5d89ea3 117
alecguertin 15:14d4e7021125 118 if (pos != -1) {
alecguertin 15:14d4e7021125 119 timer.start();
lsaristo 21:0c80a5d89ea3 120 pi.forward(CAL_SPEED);
alecguertin 15:14d4e7021125 121 } else {
alecguertin 15:14d4e7021125 122 pi.cls();
alecguertin 15:14d4e7021125 123 pi.locate(0,0);
alecguertin 15:14d4e7021125 124 pi.printf("LP: %f",pos);
lsaristo 21:0c80a5d89ea3 125 forward(1000);
lsaristo 21:0c80a5d89ea3 126 goto calibrate;
alecguertin 15:14d4e7021125 127 return 1;
alecguertin 15:14d4e7021125 128 }
lsaristo 21:0c80a5d89ea3 129 /**/
lsaristo 21:0c80a5d89ea3 130
lsaristo 21:0c80a5d89ea3 131 // wait(1);
lsaristo 21:0c80a5d89ea3 132
lsaristo 21:0c80a5d89ea3 133 do {
lsaristo 21:0c80a5d89ea3 134 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 135 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 136 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 137 pi.left_motor(CAL_SPEED-correction);
lsaristo 21:0c80a5d89ea3 138 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 139 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 140 pi.right_motor(CAL_SPEED-correction);
lsaristo 21:0c80a5d89ea3 141 } else {
lsaristo 21:0c80a5d89ea3 142 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 143 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 144 }
lsaristo 21:0c80a5d89ea3 145 pi.cls();
lsaristo 21:0c80a5d89ea3 146 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 147 pi.printf("Pos: %f", pos);
lsaristo 21:0c80a5d89ea3 148 } while(pos != -1 && pos > -0.3);
lsaristo 21:0c80a5d89ea3 149 pi.stop();
lsaristo 21:0c80a5d89ea3 150 if(pos != -1) {
lsaristo 21:0c80a5d89ea3 151 oled_1 = 1;
lsaristo 21:0c80a5d89ea3 152 timer.stop();
lsaristo 21:0c80a5d89ea3 153 pi.printf("T: %d", timer.read_ms());
lsaristo 21:0c80a5d89ea3 154 } else {
lsaristo 21:0c80a5d89ea3 155 pi.cls();
lsaristo 21:0c80a5d89ea3 156 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 157 pi.printf("lP:%f", pos);
lsaristo 21:0c80a5d89ea3 158 return 1;
lsaristo 21:0c80a5d89ea3 159 }
lsaristo 21:0c80a5d89ea3 160 right(180);
alecguertin 15:14d4e7021125 161 wait(1);
lsaristo 12:1aa6b8a74136 162 do {
lsaristo 12:1aa6b8a74136 163 pos = pi.line_position();
lsaristo 13:070846d87d4a 164 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 165 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 166 pi.left_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 167 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 168 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 169 pi.right_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 170 } else {
lsaristo 21:0c80a5d89ea3 171 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 172 pi.left_motor(CAL_SPEED);
alecguertin 15:14d4e7021125 173 }
alecguertin 15:14d4e7021125 174 pi.cls();
alecguertin 15:14d4e7021125 175 pi.locate(0,0);
alecguertin 15:14d4e7021125 176 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 177 } while(pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 178 pi.stop();
alecguertin 15:14d4e7021125 179 if(pos != -1) {
alecguertin 15:14d4e7021125 180 oled_1 = 1;
alecguertin 15:14d4e7021125 181 timer.stop();
alecguertin 15:14d4e7021125 182 pi.printf("T: %d", timer.read_ms());
alecguertin 15:14d4e7021125 183 } else {
alecguertin 15:14d4e7021125 184 pi.cls();
alecguertin 15:14d4e7021125 185 pi.locate(0,0);
alecguertin 15:14d4e7021125 186 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 187 return 1;
alecguertin 15:14d4e7021125 188 }
lsaristo 21:0c80a5d89ea3 189 right(180);
lsaristo 21:0c80a5d89ea3 190 timerWait(.07);
lsaristo 21:0c80a5d89ea3 191 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 192 while(fabs(pos) > CLOSE_ENOUGH) {
lsaristo 21:0c80a5d89ea3 193 pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
lsaristo 21:0c80a5d89ea3 194 timerWait(.08);
lsaristo 21:0c80a5d89ea3 195 pi.cls();
lsaristo 21:0c80a5d89ea3 196 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 197 pi.printf("O: %f", pos);
lsaristo 21:0c80a5d89ea3 198 pi.stop();
lsaristo 21:0c80a5d89ea3 199 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 200 timerWait(.2);
lsaristo 21:0c80a5d89ea3 201 }
lsaristo 21:0c80a5d89ea3 202 timerWait(2);
lsaristo 21:0c80a5d89ea3 203
lsaristo 21:0c80a5d89ea3 204 //
lsaristo 21:0c80a5d89ea3 205 // Pivot 180 degrees to go to the starting position.
lsaristo 21:0c80a5d89ea3 206 /*
alecguertin 15:14d4e7021125 207 do {
alecguertin 15:14d4e7021125 208 pos = pi.line_position();
alecguertin 15:14d4e7021125 209 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 210 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 211 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 212 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 213 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 214 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 215 } else {
lsaristo 21:0c80a5d89ea3 216 pi.backward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 217 }
lsaristo 12:1aa6b8a74136 218 pi.cls();
lsaristo 12:1aa6b8a74136 219 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 220 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 221 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 222 pi.stop();
alecguertin 15:14d4e7021125 223 if(pos != -1) {
alecguertin 15:14d4e7021125 224 oled_1 = 1;
alecguertin 15:14d4e7021125 225 timer.stop();
alecguertin 19:47759cf4f9b9 226 cal_time = timer.read();
chstrchu 20:76718145b403 227 pi.cls();
chstrchu 20:76718145b403 228 pi.locate(0,0);
alecguertin 15:14d4e7021125 229 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 230 } else {
lsaristo 12:1aa6b8a74136 231 pi.cls();
lsaristo 12:1aa6b8a74136 232 pi.locate(0,0);
alecguertin 15:14d4e7021125 233 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 234 return 1;
lsaristo 12:1aa6b8a74136 235 }
lsaristo 21:0c80a5d89ea3 236 /**/
chstrchu 20:76718145b403 237 /*
lsaristo 12:1aa6b8a74136 238 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 239 do {
lsaristo 12:1aa6b8a74136 240 pos = pi.line_position();
lsaristo 13:070846d87d4a 241 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 242 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 243 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 244 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 245 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 246 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 247 } else {
lsaristo 13:070846d87d4a 248 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 249 }
lsaristo 12:1aa6b8a74136 250 pi.cls();
lsaristo 12:1aa6b8a74136 251 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 252 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 253 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 254 if(pos == 1) {
lsaristo 12:1aa6b8a74136 255 oled_1 = 1;
lsaristo 12:1aa6b8a74136 256 timer.stop();
alecguertin 19:47759cf4f9b9 257 pi.stop();
lsaristo 12:1aa6b8a74136 258 } else {
lsaristo 12:1aa6b8a74136 259 pi.stop();
lsaristo 12:1aa6b8a74136 260 pi.cls();
lsaristo 12:1aa6b8a74136 261 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 262 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 263 while(1);
lsaristo 12:1aa6b8a74136 264 }
chstrchu 20:76718145b403 265 */
lsaristo 14:41fa8b95a9ab 266 // If we got here, calibration is complete.
lsaristo 12:1aa6b8a74136 267
John Wilkey 6:00b7198f0b51 268 //
lsaristo 9:3a0433c391cb 269 // Main program loop.
lsaristo 12:1aa6b8a74136 270 // robot_loop();
alecguertin 17:c72c092fcdf7 271 size_t bytes_read = 0;
alecguertin 19:47759cf4f9b9 272 int err, x, y, last_x, last_y, delta_x, delta_y, delta_a;
alecguertin 19:47759cf4f9b9 273 int dim_x, dim_y;
alecguertin 17:c72c092fcdf7 274 int offset = 0;
alecguertin 17:c72c092fcdf7 275 char *cur, *next;
alecguertin 19:47759cf4f9b9 276 float angle;
alecguertin 19:47759cf4f9b9 277 double dist, theta;
alecguertin 18:eab7b0e89398 278 angle = 0;
alecguertin 18:eab7b0e89398 279 theta = 0;
alecguertin 19:47759cf4f9b9 280 last_x = 0;
alecguertin 19:47759cf4f9b9 281 last_y = 0;
alecguertin 15:14d4e7021125 282 ps_file = fopen("/local/test.ps", "r");
chstrchu 20:76718145b403 283 if (ps_file == NULL) {
chstrchu 20:76718145b403 284 return 1;
chstrchu 20:76718145b403 285 }
chstrchu 20:76718145b403 286 pi.cls();
chstrchu 20:76718145b403 287 pi.locate(0,0);
chstrchu 20:76718145b403 288 pi.printf("open");
alecguertin 17:c72c092fcdf7 289 /* PS parsing loop. */
alecguertin 19:47759cf4f9b9 290 memset(instbuf, 0, instbuflen);
chstrchu 20:76718145b403 291 bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
chstrchu 20:76718145b403 292 pi.cls();
chstrchu 20:76718145b403 293 pi.locate(0,0);
chstrchu 20:76718145b403 294 pi.printf("%.7s", instbuf);
alecguertin 19:47759cf4f9b9 295 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 296 if (err != 2) {
chstrchu 20:76718145b403 297 pi.cls();
chstrchu 20:76718145b403 298 pi.locate(0,0);
chstrchu 20:76718145b403 299 pi.printf("sscanf1");
alecguertin 19:47759cf4f9b9 300 return 1;
alecguertin 19:47759cf4f9b9 301 }
alecguertin 19:47759cf4f9b9 302 cur = strchr(instbuf, '\n');
chstrchu 20:76718145b403 303 cur++;
chstrchu 20:76718145b403 304 offset = instbuf+instbuflen-cur;
chstrchu 20:76718145b403 305 memcpy(instbuf, cur, offset);
alecguertin 17:c72c092fcdf7 306 while (1) {
alecguertin 17:c72c092fcdf7 307 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 308 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 17:c72c092fcdf7 309 if (bytes_read == 0) {
chstrchu 20:76718145b403 310 pi.cls();
chstrchu 20:76718145b403 311 pi.locate(0,0);
chstrchu 20:76718145b403 312 pi.printf("bytes0");
alecguertin 17:c72c092fcdf7 313 break;
alecguertin 17:c72c092fcdf7 314 }
alecguertin 17:c72c092fcdf7 315 cur = instbuf;
alecguertin 17:c72c092fcdf7 316 while (cur[0] != '\0') {
alecguertin 17:c72c092fcdf7 317 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 19:47759cf4f9b9 318 if (err == 0) {
chstrchu 20:76718145b403 319 pi.cls();
chstrchu 20:76718145b403 320 pi.locate(0,0);
chstrchu 20:76718145b403 321 pi.printf("noinst");
alecguertin 19:47759cf4f9b9 322 return 1;
alecguertin 19:47759cf4f9b9 323 }
alecguertin 18:eab7b0e89398 324 delta_x = x-last_x;
alecguertin 18:eab7b0e89398 325 delta_y = y-last_y;
alecguertin 19:47759cf4f9b9 326
alecguertin 19:47759cf4f9b9 327 /* Compute turn angle and turn. */
alecguertin 19:47759cf4f9b9 328 theta = tan((double) delta_x/delta_y);
alecguertin 18:eab7b0e89398 329 theta *= 57.2957795;
alecguertin 18:eab7b0e89398 330 delta_a = theta-angle;
chstrchu 20:76718145b403 331 pi.cls();
chstrchu 20:76718145b403 332 pi.locate(0,0);
chstrchu 20:76718145b403 333 pi.printf("%d", delta_a);
alecguertin 19:47759cf4f9b9 334 if (delta_a > 0) {
chstrchu 20:76718145b403 335 left(delta_a);
alecguertin 19:47759cf4f9b9 336 } else {
chstrchu 20:76718145b403 337 right(delta_a);
alecguertin 19:47759cf4f9b9 338 }
chstrchu 20:76718145b403 339
alecguertin 19:47759cf4f9b9 340 /* Put pen into position. */
alecguertin 19:47759cf4f9b9 341 if (draw) {
alecguertin 19:47759cf4f9b9 342 oled_1 = 1;
alecguertin 19:47759cf4f9b9 343 } else {
alecguertin 19:47759cf4f9b9 344 oled_1 = 0;
alecguertin 19:47759cf4f9b9 345 }
chstrchu 20:76718145b403 346
alecguertin 19:47759cf4f9b9 347 /* Compute drive time and move forward. */
alecguertin 19:47759cf4f9b9 348 dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
alecguertin 19:47759cf4f9b9 349 if (dist < 0) {
alecguertin 19:47759cf4f9b9 350 dist *= -1;
alecguertin 19:47759cf4f9b9 351 }
lsaristo 21:0c80a5d89ea3 352 timerWait(.2);
lsaristo 21:0c80a5d89ea3 353 forward(1500);
lsaristo 21:0c80a5d89ea3 354 right(90);
lsaristo 21:0c80a5d89ea3 355 timerWait(.2);
lsaristo 21:0c80a5d89ea3 356 forward(1500);
lsaristo 21:0c80a5d89ea3 357 right(90);
lsaristo 21:0c80a5d89ea3 358 timerWait(.2);
lsaristo 21:0c80a5d89ea3 359 forward(1500);
lsaristo 21:0c80a5d89ea3 360 right(90);
lsaristo 21:0c80a5d89ea3 361 timerWait(.2);
lsaristo 21:0c80a5d89ea3 362 forward(1500);
lsaristo 21:0c80a5d89ea3 363 right(90);
lsaristo 21:0c80a5d89ea3 364 timerWait(.2);
alecguertin 19:47759cf4f9b9 365
alecguertin 17:c72c092fcdf7 366 last_x = x;
alecguertin 17:c72c092fcdf7 367 last_y = y;
alecguertin 17:c72c092fcdf7 368 next = strchr(cur, '\n');
alecguertin 17:c72c092fcdf7 369 if (next == NULL) {
chstrchu 20:76718145b403 370 pi.cls();
chstrchu 20:76718145b403 371 pi.locate(0,0);
chstrchu 20:76718145b403 372 pi.printf("nonext");
alecguertin 17:c72c092fcdf7 373 break;
alecguertin 17:c72c092fcdf7 374 }
alecguertin 17:c72c092fcdf7 375 cur = next+1;
chstrchu 20:76718145b403 376 pi.cls();
chstrchu 20:76718145b403 377 pi.locate(0,0);
chstrchu 20:76718145b403 378 pi.printf("waiting!");
alecguertin 17:c72c092fcdf7 379 }
alecguertin 17:c72c092fcdf7 380 offset = instbuf+instbuflen-cur;
alecguertin 17:c72c092fcdf7 381 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 382 }
lsaristo 9:3a0433c391cb 383 //
lsaristo 9:3a0433c391cb 384 // We should never reach this point!
alecguertin 18:eab7b0e89398 385 //
alecguertin 18:eab7b0e89398 386 // Yes, we should...
chstrchu 20:76718145b403 387 pi.cls();
chstrchu 20:76718145b403 388 pi.locate(0,0);
chstrchu 20:76718145b403 389 pi.printf("done");
alecguertin 15:14d4e7021125 390 return 0;
John Wilkey 5:01882c3de2dc 391 }
lsaristo 8:12d780f7443e 392
alecguertin 17:c72c092fcdf7 393 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 394 {
alecguertin 17:c72c092fcdf7 395 int matches;
alecguertin 17:c72c092fcdf7 396 char *srch;
alecguertin 17:c72c092fcdf7 397 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 398 if (matches != 2) {
chstrchu 20:76718145b403 399 pi.cls();
chstrchu 20:76718145b403 400 pi.locate(0,0);
chstrchu 20:76718145b403 401 pi.printf("nomatch");
alecguertin 17:c72c092fcdf7 402 return 0;
alecguertin 17:c72c092fcdf7 403 }
alecguertin 17:c72c092fcdf7 404 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 405 srch++;
chstrchu 20:76718145b403 406 srch = strchr(srch, ' ');
alecguertin 17:c72c092fcdf7 407 srch++;
alecguertin 17:c72c092fcdf7 408 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 409 *draw = 0;
chstrchu 20:76718145b403 410 } else if (srch[0] == 'l') {
alecguertin 17:c72c092fcdf7 411 *draw = 1;
alecguertin 17:c72c092fcdf7 412 } else {
chstrchu 20:76718145b403 413 pi.cls();
chstrchu 20:76718145b403 414 pi.locate(0,0);
chstrchu 20:76718145b403 415 pi.printf("%.8s", srch);
alecguertin 17:c72c092fcdf7 416 return 0;
alecguertin 17:c72c092fcdf7 417 }
alecguertin 17:c72c092fcdf7 418 return 1;
alecguertin 17:c72c092fcdf7 419 }
alecguertin 17:c72c092fcdf7 420
lsaristo 9:3a0433c391cb 421 int forward(int amt)
lsaristo 8:12d780f7443e 422 {
lsaristo 12:1aa6b8a74136 423 Timer t;
lsaristo 12:1aa6b8a74136 424 t.start();
lsaristo 10:94b068b2ce1d 425 oled_2 = 1;
lsaristo 9:3a0433c391cb 426 pi.locate(0,0);
lsaristo 9:3a0433c391cb 427 pi.printf("Fwd %d", amt);
lsaristo 21:0c80a5d89ea3 428 pi.left_motor(DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 429 pi.right_motor(DRIVE_SPEED);
lsaristo 21:0c80a5d89ea3 430 while(t.read_ms() < (amt-500));
lsaristo 21:0c80a5d89ea3 431 pi.left_motor(.7*DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 432 pi.right_motor(.7*DRIVE_SPEED);
chstrchu 20:76718145b403 433 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 434 t.stop();
lsaristo 10:94b068b2ce1d 435 oled_2 = 0;
lsaristo 12:1aa6b8a74136 436 pi.stop();
lsaristo 8:12d780f7443e 437 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 438 }
lsaristo 8:12d780f7443e 439
lsaristo 9:3a0433c391cb 440 int backward(int amt)
lsaristo 8:12d780f7443e 441 {
lsaristo 12:1aa6b8a74136 442 Timer t;
lsaristo 10:94b068b2ce1d 443 oled_3 = 1;
lsaristo 9:3a0433c391cb 444 pi.locate(0,0);
lsaristo 9:3a0433c391cb 445 pi.printf("Back %d", amt);
lsaristo 9:3a0433c391cb 446 pi.backward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 447 t.start();
chstrchu 20:76718145b403 448 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 449 t.stop();
lsaristo 10:94b068b2ce1d 450 oled_3 = 0;
lsaristo 12:1aa6b8a74136 451 pi.stop();
lsaristo 8:12d780f7443e 452 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 453 }
lsaristo 8:12d780f7443e 454
lsaristo 8:12d780f7443e 455 int right(float deg)
lsaristo 8:12d780f7443e 456 {
lsaristo 12:1aa6b8a74136 457 Timer t;
lsaristo 10:94b068b2ce1d 458 oled_4 = 1;
lsaristo 9:3a0433c391cb 459 pi.locate(0,0);
lsaristo 9:3a0433c391cb 460 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 461 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 462 t.start();
lsaristo 21:0c80a5d89ea3 463 while(t.read_ms() < (deg/360)*TIME_FACT);
lsaristo 12:1aa6b8a74136 464 t.stop();
lsaristo 10:94b068b2ce1d 465 oled_4 = 0;
lsaristo 12:1aa6b8a74136 466 pi.stop();
lsaristo 8:12d780f7443e 467 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 468 }
lsaristo 8:12d780f7443e 469
lsaristo 21:0c80a5d89ea3 470 void timerWait(float seconds)
lsaristo 21:0c80a5d89ea3 471 {
lsaristo 21:0c80a5d89ea3 472 Timer t;
lsaristo 21:0c80a5d89ea3 473 t.start();
lsaristo 21:0c80a5d89ea3 474 while(t.read_us() < 1000000*seconds);
lsaristo 21:0c80a5d89ea3 475 t.stop();
lsaristo 21:0c80a5d89ea3 476 }
lsaristo 21:0c80a5d89ea3 477
lsaristo 8:12d780f7443e 478 int left(float deg)
lsaristo 8:12d780f7443e 479 {
lsaristo 12:1aa6b8a74136 480 Timer t;
lsaristo 10:94b068b2ce1d 481 oled_4 = 1;
lsaristo 10:94b068b2ce1d 482 oled_2 = 1;
lsaristo 9:3a0433c391cb 483 pi.locate(0,0);
lsaristo 9:3a0433c391cb 484 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 485 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 486 t.start();
lsaristo 21:0c80a5d89ea3 487 while(t.read_ms() < (deg/360)*TIME_FACT);
lsaristo 12:1aa6b8a74136 488 t.stop();
lsaristo 10:94b068b2ce1d 489 oled_4 = 0;
lsaristo 10:94b068b2ce1d 490 oled_2 = 0;
lsaristo 12:1aa6b8a74136 491 pi.stop();
lsaristo 8:12d780f7443e 492 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 493 }
lsaristo 10:94b068b2ce1d 494
lsaristo 10:94b068b2ce1d 495 void pen_down()
lsaristo 10:94b068b2ce1d 496 {
lsaristo 10:94b068b2ce1d 497 oled_1 = 1;
lsaristo 10:94b068b2ce1d 498 }
lsaristo 10:94b068b2ce1d 499
lsaristo 10:94b068b2ce1d 500 void pen_up()
lsaristo 10:94b068b2ce1d 501 {
lsaristo 10:94b068b2ce1d 502 oled_1 = 0;
lsaristo 9:3a0433c391cb 503 }