EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@22:46b9d9b2e35c, 2014-12-07 (annotated)

Committer:
alecguertin
Date:
Sun Dec 07 04:39:33 2014 +0000
Revision:
22:46b9d9b2e35c
Parent:
21:0c80a5d89ea3
Child:
23:e4616259a7f0
reads instructions correctly

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 22:46b9d9b2e35c 161 Timer caltimer;
alecguertin 22:46b9d9b2e35c 162 caltimer.start();
lsaristo 12:1aa6b8a74136 163 do {
lsaristo 12:1aa6b8a74136 164 pos = pi.line_position();
lsaristo 13:070846d87d4a 165 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 166 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 167 pi.left_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 168 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 169 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 170 pi.right_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 171 } else {
lsaristo 21:0c80a5d89ea3 172 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 173 pi.left_motor(CAL_SPEED);
alecguertin 15:14d4e7021125 174 }
alecguertin 15:14d4e7021125 175 pi.cls();
alecguertin 15:14d4e7021125 176 pi.locate(0,0);
alecguertin 15:14d4e7021125 177 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 178 } while(pos != -1 && pos <= 0.3);
alecguertin 22:46b9d9b2e35c 179 caltimer.stop();
alecguertin 22:46b9d9b2e35c 180 cal_time = caltimer.read_ms();
alecguertin 15:14d4e7021125 181 pi.stop();
alecguertin 15:14d4e7021125 182 if(pos != -1) {
alecguertin 15:14d4e7021125 183 oled_1 = 1;
alecguertin 15:14d4e7021125 184 timer.stop();
alecguertin 15:14d4e7021125 185 pi.printf("T: %d", timer.read_ms());
alecguertin 15:14d4e7021125 186 } else {
alecguertin 15:14d4e7021125 187 pi.cls();
alecguertin 15:14d4e7021125 188 pi.locate(0,0);
alecguertin 15:14d4e7021125 189 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 190 return 1;
alecguertin 15:14d4e7021125 191 }
lsaristo 21:0c80a5d89ea3 192 right(180);
alecguertin 22:46b9d9b2e35c 193 timerWait(.5);
alecguertin 22:46b9d9b2e35c 194 while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
alecguertin 22:46b9d9b2e35c 195 pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 196 // timerWait(.08);
alecguertin 22:46b9d9b2e35c 197 pi.cls();
alecguertin 22:46b9d9b2e35c 198 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 199 pi.printf("O: %f", pos);
alecguertin 22:46b9d9b2e35c 200 // pi.stop();
alecguertin 22:46b9d9b2e35c 201 // timerWait(.2);
alecguertin 22:46b9d9b2e35c 202 }
alecguertin 22:46b9d9b2e35c 203 pi.stop();
alecguertin 22:46b9d9b2e35c 204 timerWait(2);
alecguertin 22:46b9d9b2e35c 205 /*
alecguertin 22:46b9d9b2e35c 206 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 207 do {
alecguertin 22:46b9d9b2e35c 208 pi.backward(CAL_SPEED);
alecguertin 22:46b9d9b2e35c 209 pi.cls();
alecguertin 22:46b9d9b2e35c 210 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 211 pi.printf("p:%f", pos);
alecguertin 22:46b9d9b2e35c 212 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 213 } while (pos != -1 && pos >= -0.25);
alecguertin 22:46b9d9b2e35c 214 /**/
alecguertin 22:46b9d9b2e35c 215 backward(500);
alecguertin 22:46b9d9b2e35c 216 timerWait(.5);
alecguertin 22:46b9d9b2e35c 217 while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
alecguertin 22:46b9d9b2e35c 218 pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 219 // timerWait(.08);
alecguertin 22:46b9d9b2e35c 220 pi.cls();
alecguertin 22:46b9d9b2e35c 221 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 222 pi.printf("O: %f", pos);
alecguertin 22:46b9d9b2e35c 223 // pi.stop();
alecguertin 22:46b9d9b2e35c 224 // timerWait(.2);
alecguertin 22:46b9d9b2e35c 225 }
alecguertin 22:46b9d9b2e35c 226 pi.stop();
alecguertin 22:46b9d9b2e35c 227 timerWait(2);
alecguertin 22:46b9d9b2e35c 228 /*
alecguertin 22:46b9d9b2e35c 229 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 230 do {
alecguertin 22:46b9d9b2e35c 231 pi.forward(CAL_SPEED);
alecguertin 22:46b9d9b2e35c 232 pi.cls();
alecguertin 22:46b9d9b2e35c 233 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 234 pi.printf("p:%f", pos);
alecguertin 22:46b9d9b2e35c 235 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 236 } while (pos <= -0.07);
alecguertin 22:46b9d9b2e35c 237 timerWait(0.25);
alecguertin 22:46b9d9b2e35c 238 pi.stop();
lsaristo 21:0c80a5d89ea3 239 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 240 while(fabs(pos) > CLOSE_ENOUGH) {
lsaristo 21:0c80a5d89ea3 241 pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
lsaristo 21:0c80a5d89ea3 242 timerWait(.08);
lsaristo 21:0c80a5d89ea3 243 pi.cls();
lsaristo 21:0c80a5d89ea3 244 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 245 pi.printf("O: %f", pos);
lsaristo 21:0c80a5d89ea3 246 pi.stop();
lsaristo 21:0c80a5d89ea3 247 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 248 timerWait(.2);
lsaristo 21:0c80a5d89ea3 249 }
alecguertin 22:46b9d9b2e35c 250 /**/
alecguertin 22:46b9d9b2e35c 251 timerWait(1);
lsaristo 21:0c80a5d89ea3 252 //
lsaristo 21:0c80a5d89ea3 253 // Pivot 180 degrees to go to the starting position.
lsaristo 21:0c80a5d89ea3 254 /*
alecguertin 15:14d4e7021125 255 do {
alecguertin 15:14d4e7021125 256 pos = pi.line_position();
alecguertin 15:14d4e7021125 257 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 258 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 259 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 260 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 261 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 262 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 263 } else {
lsaristo 21:0c80a5d89ea3 264 pi.backward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 265 }
lsaristo 12:1aa6b8a74136 266 pi.cls();
lsaristo 12:1aa6b8a74136 267 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 268 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 269 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 270 pi.stop();
alecguertin 15:14d4e7021125 271 if(pos != -1) {
alecguertin 15:14d4e7021125 272 oled_1 = 1;
alecguertin 15:14d4e7021125 273 timer.stop();
alecguertin 19:47759cf4f9b9 274 cal_time = timer.read();
chstrchu 20:76718145b403 275 pi.cls();
chstrchu 20:76718145b403 276 pi.locate(0,0);
alecguertin 15:14d4e7021125 277 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 278 } else {
lsaristo 12:1aa6b8a74136 279 pi.cls();
lsaristo 12:1aa6b8a74136 280 pi.locate(0,0);
alecguertin 15:14d4e7021125 281 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 282 return 1;
lsaristo 12:1aa6b8a74136 283 }
lsaristo 21:0c80a5d89ea3 284 /**/
chstrchu 20:76718145b403 285 /*
lsaristo 12:1aa6b8a74136 286 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 287 do {
lsaristo 12:1aa6b8a74136 288 pos = pi.line_position();
lsaristo 13:070846d87d4a 289 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 290 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 291 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 292 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 293 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 294 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 295 } else {
lsaristo 13:070846d87d4a 296 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 297 }
lsaristo 12:1aa6b8a74136 298 pi.cls();
lsaristo 12:1aa6b8a74136 299 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 300 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 301 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 302 if(pos == 1) {
lsaristo 12:1aa6b8a74136 303 oled_1 = 1;
lsaristo 12:1aa6b8a74136 304 timer.stop();
alecguertin 19:47759cf4f9b9 305 pi.stop();
lsaristo 12:1aa6b8a74136 306 } else {
lsaristo 12:1aa6b8a74136 307 pi.stop();
lsaristo 12:1aa6b8a74136 308 pi.cls();
lsaristo 12:1aa6b8a74136 309 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 310 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 311 while(1);
lsaristo 12:1aa6b8a74136 312 }
chstrchu 20:76718145b403 313 */
lsaristo 14:41fa8b95a9ab 314 // If we got here, calibration is complete.
lsaristo 12:1aa6b8a74136 315
John Wilkey 6:00b7198f0b51 316 //
lsaristo 9:3a0433c391cb 317 // Main program loop.
lsaristo 12:1aa6b8a74136 318 // robot_loop();
alecguertin 17:c72c092fcdf7 319 size_t bytes_read = 0;
alecguertin 22:46b9d9b2e35c 320 int err, x, y, last_x, last_y, delta_x, delta_y;
alecguertin 22:46b9d9b2e35c 321 float delta_a;
alecguertin 19:47759cf4f9b9 322 int dim_x, dim_y;
alecguertin 17:c72c092fcdf7 323 int offset = 0;
alecguertin 17:c72c092fcdf7 324 char *cur, *next;
alecguertin 19:47759cf4f9b9 325 float angle;
alecguertin 19:47759cf4f9b9 326 double dist, theta;
alecguertin 18:eab7b0e89398 327 angle = 0;
alecguertin 18:eab7b0e89398 328 theta = 0;
alecguertin 19:47759cf4f9b9 329 last_x = 0;
alecguertin 19:47759cf4f9b9 330 last_y = 0;
alecguertin 15:14d4e7021125 331 ps_file = fopen("/local/test.ps", "r");
chstrchu 20:76718145b403 332 if (ps_file == NULL) {
chstrchu 20:76718145b403 333 return 1;
chstrchu 20:76718145b403 334 }
chstrchu 20:76718145b403 335 pi.cls();
chstrchu 20:76718145b403 336 pi.locate(0,0);
chstrchu 20:76718145b403 337 pi.printf("open");
alecguertin 17:c72c092fcdf7 338 /* PS parsing loop. */
alecguertin 19:47759cf4f9b9 339 memset(instbuf, 0, instbuflen);
chstrchu 20:76718145b403 340 bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
alecguertin 22:46b9d9b2e35c 341 if (bytes_read == 0) {
alecguertin 22:46b9d9b2e35c 342 return 1;
alecguertin 22:46b9d9b2e35c 343 }
chstrchu 20:76718145b403 344 pi.cls();
chstrchu 20:76718145b403 345 pi.locate(0,0);
chstrchu 20:76718145b403 346 pi.printf("%.7s", instbuf);
alecguertin 19:47759cf4f9b9 347 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 348 if (err != 2) {
chstrchu 20:76718145b403 349 pi.cls();
chstrchu 20:76718145b403 350 pi.locate(0,0);
chstrchu 20:76718145b403 351 pi.printf("sscanf1");
alecguertin 19:47759cf4f9b9 352 return 1;
alecguertin 19:47759cf4f9b9 353 }
alecguertin 19:47759cf4f9b9 354 cur = strchr(instbuf, '\n');
chstrchu 20:76718145b403 355 cur++;
alecguertin 22:46b9d9b2e35c 356 offset = instbuf+instbuflen-cur-1;
chstrchu 20:76718145b403 357 memcpy(instbuf, cur, offset);
alecguertin 17:c72c092fcdf7 358 while (1) {
alecguertin 17:c72c092fcdf7 359 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 360 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 17:c72c092fcdf7 361 if (bytes_read == 0) {
chstrchu 20:76718145b403 362 pi.cls();
chstrchu 20:76718145b403 363 pi.locate(0,0);
chstrchu 20:76718145b403 364 pi.printf("bytes0");
alecguertin 22:46b9d9b2e35c 365 timerWait(2);
alecguertin 17:c72c092fcdf7 366 }
alecguertin 17:c72c092fcdf7 367 cur = instbuf;
alecguertin 22:46b9d9b2e35c 368 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 22:46b9d9b2e35c 369 if (err == 0) {
chstrchu 20:76718145b403 370 pi.cls();
chstrchu 20:76718145b403 371 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 372 pi.printf("noinst");
alecguertin 22:46b9d9b2e35c 373 return 1;
alecguertin 22:46b9d9b2e35c 374 }
alecguertin 22:46b9d9b2e35c 375 delta_x = x-last_x;
alecguertin 22:46b9d9b2e35c 376 delta_y = y-last_y;
alecguertin 22:46b9d9b2e35c 377
alecguertin 22:46b9d9b2e35c 378 /* Compute turn angle and turn. */
alecguertin 22:46b9d9b2e35c 379 theta = atan(((double) delta_x)/((double) delta_y));
alecguertin 22:46b9d9b2e35c 380 theta *= 57.29;
alecguertin 22:46b9d9b2e35c 381 delta_a = theta-angle;
alecguertin 22:46b9d9b2e35c 382 pi.cls();
alecguertin 22:46b9d9b2e35c 383 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 384 pi.printf("a:%f", delta_a);
alecguertin 22:46b9d9b2e35c 385 timerWait(2);
alecguertin 22:46b9d9b2e35c 386 if (delta_a > 0) {
alecguertin 22:46b9d9b2e35c 387 left(delta_a);
alecguertin 22:46b9d9b2e35c 388 }
alecguertin 19:47759cf4f9b9 389
alecguertin 22:46b9d9b2e35c 390 /* Put pen into position. */
alecguertin 22:46b9d9b2e35c 391 if (draw) {
alecguertin 22:46b9d9b2e35c 392 oled_1 = 1;
alecguertin 22:46b9d9b2e35c 393 } else {
alecguertin 22:46b9d9b2e35c 394 oled_1 = 0;
alecguertin 22:46b9d9b2e35c 395 }
alecguertin 22:46b9d9b2e35c 396
alecguertin 22:46b9d9b2e35c 397 /* Compute drive time and move forward. */
alecguertin 22:46b9d9b2e35c 398 dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
alecguertin 22:46b9d9b2e35c 399 if (dist < 0) {
alecguertin 22:46b9d9b2e35c 400 dist *= -1;
alecguertin 22:46b9d9b2e35c 401 }
alecguertin 22:46b9d9b2e35c 402 pi.cls();
alecguertin 22:46b9d9b2e35c 403 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 404 pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
alecguertin 22:46b9d9b2e35c 405 timerWait(2);
alecguertin 22:46b9d9b2e35c 406 forward(0.4*cal_time*(dist/(double)dim_x));
alecguertin 22:46b9d9b2e35c 407
alecguertin 22:46b9d9b2e35c 408 last_x = x;
alecguertin 22:46b9d9b2e35c 409 last_y = y;
alecguertin 22:46b9d9b2e35c 410 next = strchr(cur, '\n');
alecguertin 22:46b9d9b2e35c 411 if (next == NULL) {
chstrchu 20:76718145b403 412 pi.cls();
chstrchu 20:76718145b403 413 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 414 pi.printf("nonext");
alecguertin 22:46b9d9b2e35c 415 break;
alecguertin 17:c72c092fcdf7 416 }
alecguertin 22:46b9d9b2e35c 417 cur = next+1;
alecguertin 22:46b9d9b2e35c 418 pi.cls();
alecguertin 22:46b9d9b2e35c 419 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 420 pi.printf("waiting!");
alecguertin 22:46b9d9b2e35c 421 offset = instbuf+instbuflen-cur-1;
alecguertin 17:c72c092fcdf7 422 memcpy(instbuf, cur, offset);
alecguertin 22:46b9d9b2e35c 423 pi.cls();
alecguertin 22:46b9d9b2e35c 424 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 425 pi.printf("%.08s", instbuf);
alecguertin 22:46b9d9b2e35c 426 timerWait(2);
alecguertin 15:14d4e7021125 427 }
lsaristo 9:3a0433c391cb 428 //
lsaristo 9:3a0433c391cb 429 // We should never reach this point!
alecguertin 18:eab7b0e89398 430 //
alecguertin 18:eab7b0e89398 431 // Yes, we should...
chstrchu 20:76718145b403 432 pi.cls();
chstrchu 20:76718145b403 433 pi.locate(0,0);
chstrchu 20:76718145b403 434 pi.printf("done");
alecguertin 15:14d4e7021125 435 return 0;
John Wilkey 5:01882c3de2dc 436 }
lsaristo 8:12d780f7443e 437
alecguertin 17:c72c092fcdf7 438 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 439 {
alecguertin 17:c72c092fcdf7 440 int matches;
alecguertin 17:c72c092fcdf7 441 char *srch;
alecguertin 17:c72c092fcdf7 442 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 443 if (matches != 2) {
chstrchu 20:76718145b403 444 pi.cls();
chstrchu 20:76718145b403 445 pi.locate(0,0);
chstrchu 20:76718145b403 446 pi.printf("nomatch");
alecguertin 17:c72c092fcdf7 447 return 0;
alecguertin 17:c72c092fcdf7 448 }
alecguertin 17:c72c092fcdf7 449 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 450 srch++;
chstrchu 20:76718145b403 451 srch = strchr(srch, ' ');
alecguertin 17:c72c092fcdf7 452 srch++;
alecguertin 17:c72c092fcdf7 453 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 454 *draw = 0;
chstrchu 20:76718145b403 455 } else if (srch[0] == 'l') {
alecguertin 17:c72c092fcdf7 456 *draw = 1;
alecguertin 17:c72c092fcdf7 457 } else {
chstrchu 20:76718145b403 458 pi.cls();
chstrchu 20:76718145b403 459 pi.locate(0,0);
chstrchu 20:76718145b403 460 pi.printf("%.8s", srch);
alecguertin 17:c72c092fcdf7 461 return 0;
alecguertin 17:c72c092fcdf7 462 }
alecguertin 17:c72c092fcdf7 463 return 1;
alecguertin 17:c72c092fcdf7 464 }
alecguertin 17:c72c092fcdf7 465
lsaristo 9:3a0433c391cb 466 int forward(int amt)
lsaristo 8:12d780f7443e 467 {
lsaristo 12:1aa6b8a74136 468 Timer t;
lsaristo 12:1aa6b8a74136 469 t.start();
lsaristo 10:94b068b2ce1d 470 oled_2 = 1;
lsaristo 9:3a0433c391cb 471 pi.locate(0,0);
lsaristo 9:3a0433c391cb 472 pi.printf("Fwd %d", amt);
lsaristo 21:0c80a5d89ea3 473 pi.left_motor(DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 474 pi.right_motor(DRIVE_SPEED);
lsaristo 21:0c80a5d89ea3 475 while(t.read_ms() < (amt-500));
lsaristo 21:0c80a5d89ea3 476 pi.left_motor(.7*DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 477 pi.right_motor(.7*DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 478 t.stop();
lsaristo 10:94b068b2ce1d 479 oled_2 = 0;
lsaristo 12:1aa6b8a74136 480 pi.stop();
lsaristo 8:12d780f7443e 481 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 482 }
lsaristo 8:12d780f7443e 483
lsaristo 9:3a0433c391cb 484 int backward(int amt)
lsaristo 8:12d780f7443e 485 {
lsaristo 12:1aa6b8a74136 486 Timer t;
lsaristo 10:94b068b2ce1d 487 oled_3 = 1;
lsaristo 9:3a0433c391cb 488 pi.locate(0,0);
lsaristo 9:3a0433c391cb 489 pi.printf("Back %d", amt);
lsaristo 12:1aa6b8a74136 490 t.start();
alecguertin 22:46b9d9b2e35c 491 pi.backward(.5*DRIVE_SPEED);
alecguertin 22:46b9d9b2e35c 492 while(t.read_ms() < (amt-500));
alecguertin 22:46b9d9b2e35c 493 pi.backward(.5*DRIVE_SPEED);
chstrchu 20:76718145b403 494 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 495 t.stop();
lsaristo 10:94b068b2ce1d 496 oled_3 = 0;
lsaristo 12:1aa6b8a74136 497 pi.stop();
lsaristo 8:12d780f7443e 498 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 499 }
lsaristo 8:12d780f7443e 500
lsaristo 8:12d780f7443e 501 int right(float deg)
lsaristo 8:12d780f7443e 502 {
lsaristo 12:1aa6b8a74136 503 Timer t;
lsaristo 10:94b068b2ce1d 504 oled_4 = 1;
lsaristo 9:3a0433c391cb 505 pi.locate(0,0);
lsaristo 9:3a0433c391cb 506 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 507 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 508 t.start();
alecguertin 22:46b9d9b2e35c 509 while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
lsaristo 12:1aa6b8a74136 510 t.stop();
lsaristo 10:94b068b2ce1d 511 oled_4 = 0;
lsaristo 12:1aa6b8a74136 512 pi.stop();
lsaristo 8:12d780f7443e 513 return EXIT_SUCCESS;
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);
lsaristo 21:0c80a5d89ea3 521 t.stop();
lsaristo 21:0c80a5d89ea3 522 }
lsaristo 21:0c80a5d89ea3 523
lsaristo 8:12d780f7443e 524 int left(float deg)
lsaristo 8:12d780f7443e 525 {
lsaristo 12:1aa6b8a74136 526 Timer t;
lsaristo 10:94b068b2ce1d 527 oled_4 = 1;
lsaristo 10:94b068b2ce1d 528 oled_2 = 1;
lsaristo 9:3a0433c391cb 529 pi.locate(0,0);
lsaristo 9:3a0433c391cb 530 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 531 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 532 t.start();
alecguertin 22:46b9d9b2e35c 533 while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
lsaristo 12:1aa6b8a74136 534 t.stop();
lsaristo 10:94b068b2ce1d 535 oled_4 = 0;
lsaristo 10:94b068b2ce1d 536 oled_2 = 0;
lsaristo 12:1aa6b8a74136 537 pi.stop();
lsaristo 8:12d780f7443e 538 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 539 }
lsaristo 10:94b068b2ce1d 540
lsaristo 10:94b068b2ce1d 541 void pen_down()
lsaristo 10:94b068b2ce1d 542 {
lsaristo 10:94b068b2ce1d 543 oled_1 = 1;
lsaristo 10:94b068b2ce1d 544 }
lsaristo 10:94b068b2ce1d 545
lsaristo 10:94b068b2ce1d 546 void pen_up()
lsaristo 10:94b068b2ce1d 547 {
lsaristo 10:94b068b2ce1d 548 oled_1 = 0;
lsaristo 9:3a0433c391cb 549 }