EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@25:2c7717684d09, 2014-12-08 (annotated)

Committer:
alecguertin
Date:
Mon Dec 08 04:21:55 2014 +0000
Revision:
25:2c7717684d09
Parent:
24:b797563776fc
Child:
26:a74edc4c6acb
pretty much working!

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.
alecguertin 25:2c7717684d09 26 DigitalOut pin13(p13);
alecguertin 25:2c7717684d09 27 DigitalOut pin14(p14);
lsaristo 7:6e5cc24e1ce7 28 DigitalOut pin15(p15);
lsaristo 7:6e5cc24e1ce7 29 DigitalOut pin16(p16);
lsaristo 7:6e5cc24e1ce7 30 DigitalOut pin17(p17);
lsaristo 7:6e5cc24e1ce7 31 DigitalOut pin18(p18);
lsaristo 7:6e5cc24e1ce7 32 DigitalOut pin19(p19);
lsaristo 7:6e5cc24e1ce7 33 DigitalOut pin20(p20);
lsaristo 7:6e5cc24e1ce7 34
lsaristo 9:3a0433c391cb 35 //
lsaristo 9:3a0433c391cb 36 // mBed onboard LEDs
lsaristo 9:3a0433c391cb 37 DigitalOut oled_1(LED1);
lsaristo 9:3a0433c391cb 38 DigitalOut oled_2(LED2);
lsaristo 9:3a0433c391cb 39 DigitalOut oled_3(LED3);
lsaristo 9:3a0433c391cb 40 DigitalOut oled_4(LED4);
lsaristo 9:3a0433c391cb 41
alecguertin 15:14d4e7021125 42 /* Local File System */
alecguertin 15:14d4e7021125 43 LocalFileSystem local("local");
lsaristo 9:3a0433c391cb 44
alecguertin 17:c72c092fcdf7 45 /* Boolean for drawing/moving. */
alecguertin 17:c72c092fcdf7 46 int draw;
alecguertin 17:c72c092fcdf7 47
John Wilkey 5:01882c3de2dc 48 /**
John Wilkey 5:01882c3de2dc 49 * @brief Entry point. Main loop.
John Wilkey 5:01882c3de2dc 50 */
John Wilkey 5:01882c3de2dc 51 int main()
John Wilkey 5:01882c3de2dc 52 {
alecguertin 17:c72c092fcdf7 53 FILE *ps_file;
alecguertin 17:c72c092fcdf7 54 int instbuflen = 250;
alecguertin 17:c72c092fcdf7 55 char instbuf[instbuflen];
alecguertin 17:c72c092fcdf7 56
lsaristo 9:3a0433c391cb 57 //
lsaristo 9:3a0433c391cb 58 // Basic setup information
lsaristo 10:94b068b2ce1d 59 start_button.mode(PullUp);
lsaristo 12:1aa6b8a74136 60
lsaristo 21:0c80a5d89ea3 61 pi.cls();
lsaristo 21:0c80a5d89ea3 62 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 63 pi.printf("PiCO");
lsaristo 21:0c80a5d89ea3 64 pi.locate(0,1);
lsaristo 21:0c80a5d89ea3 65 pi.printf("%f mV", pi.battery());
lsaristo 21:0c80a5d89ea3 66 wait(.5);
lsaristo 21:0c80a5d89ea3 67 /* while(start_button) {
lsaristo 21:0c80a5d89ea3 68 oled_2 = 1;
lsaristo 21:0c80a5d89ea3 69 wait(.5);
lsaristo 21:0c80a5d89ea3 70 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 71 pi.printf("Ready");
lsaristo 21:0c80a5d89ea3 72 oled_2 = 0;
lsaristo 24:b797563776fc 73 } */
lsaristo 21:0c80a5d89ea3 74 pi.cls();
lsaristo 21:0c80a5d89ea3 75 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 76 // pi.printf("GO!");
lsaristo 21:0c80a5d89ea3 77 wait(.5);
lsaristo 21:0c80a5d89ea3 78
lsaristo 21:0c80a5d89ea3 79 /*
lsaristo 21:0c80a5d89ea3 80 while(1) {
lsaristo 21:0c80a5d89ea3 81 if(!start_button) {
lsaristo 21:0c80a5d89ea3 82 pi.stop();
lsaristo 21:0c80a5d89ea3 83 goto start;
lsaristo 21:0c80a5d89ea3 84 } else {
lsaristo 21:0c80a5d89ea3 85 goto calibrate;
lsaristo 21:0c80a5d89ea3 86 }
lsaristo 21:0c80a5d89ea3 87 }
lsaristo 24:b797563776fc 88 */
lsaristo 21:0c80a5d89ea3 89
lsaristo 13:070846d87d4a 90 //
lsaristo 13:070846d87d4a 91 // Drawing environment calibration.
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 24:b797563776fc 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 timer.stop();
lsaristo 21:0c80a5d89ea3 152 } else {
lsaristo 21:0c80a5d89ea3 153 return 1;
lsaristo 21:0c80a5d89ea3 154 }
lsaristo 21:0c80a5d89ea3 155 right(180);
alecguertin 22:46b9d9b2e35c 156 Timer caltimer;
alecguertin 22:46b9d9b2e35c 157 caltimer.start();
lsaristo 12:1aa6b8a74136 158 do {
lsaristo 12:1aa6b8a74136 159 pos = pi.line_position();
lsaristo 13:070846d87d4a 160 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 161 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 162 pi.left_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 163 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 164 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 165 pi.right_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 166 } else {
lsaristo 21:0c80a5d89ea3 167 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 168 pi.left_motor(CAL_SPEED);
alecguertin 15:14d4e7021125 169 }
alecguertin 15:14d4e7021125 170 pi.cls();
alecguertin 15:14d4e7021125 171 pi.locate(0,0);
alecguertin 15:14d4e7021125 172 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 173 } while(pos != -1 && pos <= 0.3);
alecguertin 22:46b9d9b2e35c 174 caltimer.stop();
alecguertin 22:46b9d9b2e35c 175 cal_time = caltimer.read_ms();
alecguertin 15:14d4e7021125 176 pi.stop();
alecguertin 15:14d4e7021125 177 if(pos != -1) {
alecguertin 15:14d4e7021125 178 timer.stop();
alecguertin 15:14d4e7021125 179 } else {
alecguertin 15:14d4e7021125 180 return 1;
alecguertin 15:14d4e7021125 181 }
lsaristo 21:0c80a5d89ea3 182 right(180);
alecguertin 22:46b9d9b2e35c 183 timerWait(.5);
alecguertin 22:46b9d9b2e35c 184 while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
alecguertin 22:46b9d9b2e35c 185 pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 186 pi.cls();
alecguertin 22:46b9d9b2e35c 187 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 188 pi.printf("O: %f", pos);
alecguertin 22:46b9d9b2e35c 189 }
alecguertin 22:46b9d9b2e35c 190 pi.stop();
alecguertin 22:46b9d9b2e35c 191 timerWait(2);
alecguertin 22:46b9d9b2e35c 192 /*
alecguertin 22:46b9d9b2e35c 193 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 194 do {
alecguertin 22:46b9d9b2e35c 195 pi.backward(CAL_SPEED);
alecguertin 22:46b9d9b2e35c 196 pi.cls();
alecguertin 22:46b9d9b2e35c 197 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 198 pi.printf("p:%f", pos);
alecguertin 22:46b9d9b2e35c 199 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 200 } while (pos != -1 && pos >= -0.25);
lsaristo 24:b797563776fc 201 */
alecguertin 22:46b9d9b2e35c 202 backward(500);
alecguertin 22:46b9d9b2e35c 203 timerWait(.5);
alecguertin 22:46b9d9b2e35c 204 while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
alecguertin 22:46b9d9b2e35c 205 pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 206 pi.cls();
alecguertin 22:46b9d9b2e35c 207 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 208 pi.printf("O: %f", pos);
alecguertin 22:46b9d9b2e35c 209 }
alecguertin 22:46b9d9b2e35c 210 pi.stop();
alecguertin 22:46b9d9b2e35c 211 timerWait(2);
alecguertin 22:46b9d9b2e35c 212 /*
alecguertin 22:46b9d9b2e35c 213 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 214 do {
alecguertin 22:46b9d9b2e35c 215 pi.forward(CAL_SPEED);
alecguertin 22:46b9d9b2e35c 216 pi.cls();
alecguertin 22:46b9d9b2e35c 217 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 218 pi.printf("p:%f", pos);
alecguertin 22:46b9d9b2e35c 219 pos = pi.line_position();
alecguertin 22:46b9d9b2e35c 220 } while (pos <= -0.07);
alecguertin 22:46b9d9b2e35c 221 timerWait(0.25);
alecguertin 22:46b9d9b2e35c 222 pi.stop();
lsaristo 21:0c80a5d89ea3 223 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 224 while(fabs(pos) > CLOSE_ENOUGH) {
lsaristo 21:0c80a5d89ea3 225 pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
lsaristo 21:0c80a5d89ea3 226 timerWait(.08);
lsaristo 21:0c80a5d89ea3 227 pi.cls();
lsaristo 21:0c80a5d89ea3 228 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 229 pi.printf("O: %f", pos);
lsaristo 21:0c80a5d89ea3 230 pi.stop();
lsaristo 21:0c80a5d89ea3 231 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 232 timerWait(.2);
lsaristo 21:0c80a5d89ea3 233 }
lsaristo 24:b797563776fc 234 */
alecguertin 22:46b9d9b2e35c 235 timerWait(1);
lsaristo 21:0c80a5d89ea3 236 //
lsaristo 21:0c80a5d89ea3 237 // Pivot 180 degrees to go to the starting position.
lsaristo 21:0c80a5d89ea3 238 /*
alecguertin 15:14d4e7021125 239 do {
alecguertin 15:14d4e7021125 240 pos = pi.line_position();
alecguertin 15:14d4e7021125 241 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 242 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 243 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 244 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 245 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 246 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 247 } else {
lsaristo 21:0c80a5d89ea3 248 pi.backward(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("P: %f", pos);
alecguertin 15:14d4e7021125 253 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 254 pi.stop();
alecguertin 15:14d4e7021125 255 if(pos != -1) {
alecguertin 15:14d4e7021125 256 oled_1 = 1;
alecguertin 15:14d4e7021125 257 timer.stop();
alecguertin 19:47759cf4f9b9 258 cal_time = timer.read();
chstrchu 20:76718145b403 259 pi.cls();
chstrchu 20:76718145b403 260 pi.locate(0,0);
alecguertin 15:14d4e7021125 261 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 262 } else {
lsaristo 12:1aa6b8a74136 263 pi.cls();
lsaristo 12:1aa6b8a74136 264 pi.locate(0,0);
alecguertin 15:14d4e7021125 265 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 266 return 1;
lsaristo 12:1aa6b8a74136 267 }
lsaristo 24:b797563776fc 268 */
chstrchu 20:76718145b403 269 /*
lsaristo 12:1aa6b8a74136 270 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 271 do {
lsaristo 12:1aa6b8a74136 272 pos = pi.line_position();
lsaristo 13:070846d87d4a 273 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 274 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 275 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 276 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 277 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 278 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 279 } else {
lsaristo 13:070846d87d4a 280 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 281 }
lsaristo 12:1aa6b8a74136 282 pi.cls();
lsaristo 12:1aa6b8a74136 283 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 284 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 285 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 286 if(pos == 1) {
lsaristo 12:1aa6b8a74136 287 oled_1 = 1;
lsaristo 12:1aa6b8a74136 288 timer.stop();
alecguertin 19:47759cf4f9b9 289 pi.stop();
lsaristo 12:1aa6b8a74136 290 } else {
lsaristo 12:1aa6b8a74136 291 pi.stop();
lsaristo 12:1aa6b8a74136 292 pi.cls();
lsaristo 12:1aa6b8a74136 293 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 294 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 295 while(1);
lsaristo 12:1aa6b8a74136 296 }
chstrchu 20:76718145b403 297 */
lsaristo 12:1aa6b8a74136 298
John Wilkey 6:00b7198f0b51 299 //
lsaristo 9:3a0433c391cb 300 // Main program loop.
lsaristo 12:1aa6b8a74136 301 // robot_loop();
alecguertin 17:c72c092fcdf7 302 size_t bytes_read = 0;
alecguertin 22:46b9d9b2e35c 303 int err, x, y, last_x, last_y, delta_x, delta_y;
alecguertin 22:46b9d9b2e35c 304 float delta_a;
alecguertin 19:47759cf4f9b9 305 int dim_x, dim_y;
alecguertin 17:c72c092fcdf7 306 int offset = 0;
alecguertin 17:c72c092fcdf7 307 char *cur, *next;
alecguertin 19:47759cf4f9b9 308 float angle;
alecguertin 19:47759cf4f9b9 309 double dist, theta;
alecguertin 18:eab7b0e89398 310 angle = 0;
alecguertin 18:eab7b0e89398 311 theta = 0;
alecguertin 19:47759cf4f9b9 312 last_x = 0;
alecguertin 19:47759cf4f9b9 313 last_y = 0;
alecguertin 15:14d4e7021125 314 ps_file = fopen("/local/test.ps", "r");
chstrchu 20:76718145b403 315 if (ps_file == NULL) {
chstrchu 20:76718145b403 316 return 1;
chstrchu 20:76718145b403 317 }
alecguertin 25:2c7717684d09 318
alecguertin 17:c72c092fcdf7 319 /* PS parsing loop. */
alecguertin 19:47759cf4f9b9 320 memset(instbuf, 0, instbuflen);
chstrchu 20:76718145b403 321 bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
alecguertin 22:46b9d9b2e35c 322 if (bytes_read == 0) {
alecguertin 22:46b9d9b2e35c 323 return 1;
alecguertin 22:46b9d9b2e35c 324 }
alecguertin 19:47759cf4f9b9 325 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 326 if (err != 2) {
chstrchu 20:76718145b403 327 pi.cls();
chstrchu 20:76718145b403 328 pi.locate(0,0);
chstrchu 20:76718145b403 329 pi.printf("sscanf1");
alecguertin 19:47759cf4f9b9 330 return 1;
alecguertin 19:47759cf4f9b9 331 }
alecguertin 19:47759cf4f9b9 332 cur = strchr(instbuf, '\n');
chstrchu 20:76718145b403 333 cur++;
alecguertin 22:46b9d9b2e35c 334 offset = instbuf+instbuflen-cur-1;
chstrchu 20:76718145b403 335 memcpy(instbuf, cur, offset);
alecguertin 17:c72c092fcdf7 336 while (1) {
alecguertin 17:c72c092fcdf7 337 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 338 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 17:c72c092fcdf7 339 cur = instbuf;
alecguertin 22:46b9d9b2e35c 340 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 22:46b9d9b2e35c 341 if (err == 0) {
chstrchu 20:76718145b403 342 pi.cls();
chstrchu 20:76718145b403 343 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 344 pi.printf("noinst");
alecguertin 22:46b9d9b2e35c 345 return 1;
alecguertin 22:46b9d9b2e35c 346 }
alecguertin 22:46b9d9b2e35c 347 delta_x = x-last_x;
alecguertin 22:46b9d9b2e35c 348 delta_y = y-last_y;
alecguertin 22:46b9d9b2e35c 349
alecguertin 22:46b9d9b2e35c 350 /* Compute turn angle and turn. */
alecguertin 25:2c7717684d09 351 theta = atan(((double) delta_y)/((double) delta_x));
alecguertin 22:46b9d9b2e35c 352 theta *= 57.29;
alecguertin 22:46b9d9b2e35c 353 delta_a = theta-angle;
alecguertin 23:e4616259a7f0 354 if (delta_x < 0 && delta_y < 0) {
alecguertin 25:2c7717684d09 355 //pi.cls();
alecguertin 25:2c7717684d09 356 //pi.locate(0,0);
alecguertin 25:2c7717684d09 357 //pi.printf("pland");
alecguertin 25:2c7717684d09 358 //timerWait(2);
alecguertin 23:e4616259a7f0 359 delta_a += 180;
alecguertin 23:e4616259a7f0 360 }
alecguertin 25:2c7717684d09 361 angle += delta_a;
alecguertin 25:2c7717684d09 362 //pi.cls();
alecguertin 25:2c7717684d09 363 //pi.locate(0,0);
alecguertin 25:2c7717684d09 364 //pi.printf("a:%f", delta_a);
alecguertin 25:2c7717684d09 365 //timerWait(1.5);
alecguertin 25:2c7717684d09 366 left(delta_a);
alecguertin 19:47759cf4f9b9 367
alecguertin 22:46b9d9b2e35c 368 /* Put pen into position. */
alecguertin 22:46b9d9b2e35c 369 if (draw) {
alecguertin 22:46b9d9b2e35c 370 oled_1 = 1;
alecguertin 22:46b9d9b2e35c 371 } else {
alecguertin 22:46b9d9b2e35c 372 oled_1 = 0;
alecguertin 22:46b9d9b2e35c 373 }
alecguertin 22:46b9d9b2e35c 374
alecguertin 22:46b9d9b2e35c 375 /* Compute drive time and move forward. */
alecguertin 22:46b9d9b2e35c 376 dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
alecguertin 22:46b9d9b2e35c 377 if (dist < 0) {
alecguertin 22:46b9d9b2e35c 378 dist *= -1;
alecguertin 22:46b9d9b2e35c 379 }
alecguertin 25:2c7717684d09 380 //pi.cls();
alecguertin 25:2c7717684d09 381 //pi.locate(0,0);
alecguertin 25:2c7717684d09 382 //pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
alecguertin 25:2c7717684d09 383 //timerWait(1.5);
alecguertin 22:46b9d9b2e35c 384 forward(0.4*cal_time*(dist/(double)dim_x));
alecguertin 22:46b9d9b2e35c 385
alecguertin 22:46b9d9b2e35c 386 last_x = x;
alecguertin 22:46b9d9b2e35c 387 last_y = y;
alecguertin 22:46b9d9b2e35c 388 next = strchr(cur, '\n');
alecguertin 22:46b9d9b2e35c 389 if (next == NULL) {
chstrchu 20:76718145b403 390 pi.cls();
chstrchu 20:76718145b403 391 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 392 pi.printf("nonext");
alecguertin 22:46b9d9b2e35c 393 break;
alecguertin 17:c72c092fcdf7 394 }
alecguertin 22:46b9d9b2e35c 395 cur = next+1;
alecguertin 22:46b9d9b2e35c 396 offset = instbuf+instbuflen-cur-1;
alecguertin 17:c72c092fcdf7 397 memcpy(instbuf, cur, offset);
alecguertin 25:2c7717684d09 398 //pi.cls();
alecguertin 25:2c7717684d09 399 //pi.locate(0,0);
alecguertin 25:2c7717684d09 400 //pi.printf("%.08s", instbuf);
alecguertin 25:2c7717684d09 401 //timerWait(2);
alecguertin 15:14d4e7021125 402 }
chstrchu 20:76718145b403 403 pi.cls();
chstrchu 20:76718145b403 404 pi.locate(0,0);
chstrchu 20:76718145b403 405 pi.printf("done");
alecguertin 15:14d4e7021125 406 return 0;
John Wilkey 5:01882c3de2dc 407 }
lsaristo 8:12d780f7443e 408
alecguertin 17:c72c092fcdf7 409 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 410 {
alecguertin 17:c72c092fcdf7 411 int matches;
alecguertin 17:c72c092fcdf7 412 char *srch;
alecguertin 17:c72c092fcdf7 413 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 414 if (matches != 2) {
chstrchu 20:76718145b403 415 pi.cls();
chstrchu 20:76718145b403 416 pi.locate(0,0);
chstrchu 20:76718145b403 417 pi.printf("nomatch");
alecguertin 17:c72c092fcdf7 418 return 0;
alecguertin 17:c72c092fcdf7 419 }
alecguertin 17:c72c092fcdf7 420 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 421 srch++;
chstrchu 20:76718145b403 422 srch = strchr(srch, ' ');
alecguertin 17:c72c092fcdf7 423 srch++;
alecguertin 17:c72c092fcdf7 424 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 425 *draw = 0;
chstrchu 20:76718145b403 426 } else if (srch[0] == 'l') {
alecguertin 17:c72c092fcdf7 427 *draw = 1;
alecguertin 17:c72c092fcdf7 428 } else {
chstrchu 20:76718145b403 429 pi.cls();
chstrchu 20:76718145b403 430 pi.locate(0,0);
chstrchu 20:76718145b403 431 pi.printf("%.8s", srch);
alecguertin 17:c72c092fcdf7 432 return 0;
alecguertin 17:c72c092fcdf7 433 }
alecguertin 17:c72c092fcdf7 434 return 1;
alecguertin 17:c72c092fcdf7 435 }
alecguertin 17:c72c092fcdf7 436
lsaristo 9:3a0433c391cb 437 int forward(int amt)
lsaristo 8:12d780f7443e 438 {
lsaristo 12:1aa6b8a74136 439 Timer t;
lsaristo 9:3a0433c391cb 440 pi.locate(0,0);
lsaristo 9:3a0433c391cb 441 pi.printf("Fwd %d", amt);
lsaristo 21:0c80a5d89ea3 442 pi.left_motor(DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 443 pi.right_motor(DRIVE_SPEED);
alecguertin 25:2c7717684d09 444 t.start();
alecguertin 25:2c7717684d09 445 while(t.read_ms() < amt);
lsaristo 21:0c80a5d89ea3 446 pi.left_motor(.7*DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 447 pi.right_motor(.7*DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 448 t.stop();
lsaristo 12:1aa6b8a74136 449 pi.stop();
lsaristo 8:12d780f7443e 450 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 451 }
lsaristo 8:12d780f7443e 452
lsaristo 9:3a0433c391cb 453 int backward(int amt)
lsaristo 8:12d780f7443e 454 {
lsaristo 12:1aa6b8a74136 455 Timer t;
lsaristo 12:1aa6b8a74136 456 t.start();
alecguertin 22:46b9d9b2e35c 457 pi.backward(.5*DRIVE_SPEED);
alecguertin 22:46b9d9b2e35c 458 while(t.read_ms() < (amt-500));
alecguertin 22:46b9d9b2e35c 459 pi.backward(.5*DRIVE_SPEED);
chstrchu 20:76718145b403 460 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 461 t.stop();
lsaristo 12:1aa6b8a74136 462 pi.stop();
lsaristo 8:12d780f7443e 463 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 464 }
lsaristo 8:12d780f7443e 465
lsaristo 8:12d780f7443e 466 int right(float deg)
lsaristo 8:12d780f7443e 467 {
lsaristo 24:b797563776fc 468 if(deg < 0) {
lsaristo 24:b797563776fc 469 return left(-1*deg);
lsaristo 24:b797563776fc 470 }
lsaristo 12:1aa6b8a74136 471 Timer t;
lsaristo 8:12d780f7443e 472 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 473 t.start();
alecguertin 22:46b9d9b2e35c 474 while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
lsaristo 12:1aa6b8a74136 475 t.stop();
lsaristo 12:1aa6b8a74136 476 pi.stop();
lsaristo 8:12d780f7443e 477 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 478 }
lsaristo 8:12d780f7443e 479
lsaristo 21:0c80a5d89ea3 480 void timerWait(float seconds)
lsaristo 21:0c80a5d89ea3 481 {
lsaristo 21:0c80a5d89ea3 482 Timer t;
lsaristo 21:0c80a5d89ea3 483 t.start();
lsaristo 21:0c80a5d89ea3 484 while(t.read_us() < 1000000*seconds);
lsaristo 21:0c80a5d89ea3 485 t.stop();
lsaristo 21:0c80a5d89ea3 486 }
lsaristo 21:0c80a5d89ea3 487
lsaristo 8:12d780f7443e 488 int left(float deg)
lsaristo 8:12d780f7443e 489 {
lsaristo 24:b797563776fc 490 if(deg < 0) {
lsaristo 24:b797563776fc 491 return right(-1*deg);
lsaristo 24:b797563776fc 492 }
lsaristo 12:1aa6b8a74136 493 Timer t;
lsaristo 8:12d780f7443e 494 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 495 t.start();
alecguertin 22:46b9d9b2e35c 496 while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
lsaristo 12:1aa6b8a74136 497 t.stop();
lsaristo 12:1aa6b8a74136 498 pi.stop();
lsaristo 8:12d780f7443e 499 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 500 }
lsaristo 10:94b068b2ce1d 501
lsaristo 10:94b068b2ce1d 502 void pen_down()
lsaristo 10:94b068b2ce1d 503 {
lsaristo 10:94b068b2ce1d 504 oled_1 = 1;
lsaristo 10:94b068b2ce1d 505 }
lsaristo 10:94b068b2ce1d 506
lsaristo 10:94b068b2ce1d 507 void pen_up()
lsaristo 10:94b068b2ce1d 508 {
lsaristo 10:94b068b2ce1d 509 oled_1 = 0;
lsaristo 9:3a0433c391cb 510 }