EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@24:b797563776fc, 2014-12-07 (annotated)

Committer:
lsaristo
Date:
Sun Dec 07 09:36:59 2014 +0000
Revision:
24:b797563776fc
Parent:
23:e4616259a7f0
Child:
25:2c7717684d09
Changed turning functions to allow negative arguments

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