EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@17:c72c092fcdf7, 2014-12-01 (annotated)

Committer:
alecguertin
Date:
Mon Dec 01 00:41:04 2014 +0000
Revision:
17:c72c092fcdf7
Parent:
16:f1beef7beeb9
Child:
18:eab7b0e89398
added instruction parsing loop -- untested

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 *
lsaristo 1:7e0243c27ecb 8 * @author John Wilkey
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 13:070846d87d4a 59 //
lsaristo 13:070846d87d4a 60 // Drawing environment calibration.
lsaristo 12:1aa6b8a74136 61 pi.sensor_auto_calibrate();
alecguertin 15:14d4e7021125 62 //pi.backward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 63 float pos = 0;
alecguertin 15:14d4e7021125 64 float over_thresh = 0.05;
alecguertin 15:14d4e7021125 65 float correction = 0.2*DRIVE_SPEED;
alecguertin 15:14d4e7021125 66
alecguertin 15:14d4e7021125 67 wait(1);
alecguertin 15:14d4e7021125 68
alecguertin 15:14d4e7021125 69 do {
alecguertin 15:14d4e7021125 70 pos = pi.line_position();
alecguertin 15:14d4e7021125 71 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 72 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 73 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 74 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 75 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 76 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 77 } else {
alecguertin 15:14d4e7021125 78 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 79 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 80 }
alecguertin 15:14d4e7021125 81 /*
alecguertin 15:14d4e7021125 82 pi.cls();
alecguertin 15:14d4e7021125 83 pi.locate(0,0);
alecguertin 15:14d4e7021125 84 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 85 */
alecguertin 17:c72c092fcdf7 86 } while (pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 87 pi.stop();
alecguertin 15:14d4e7021125 88 wait(1);
alecguertin 15:14d4e7021125 89 if (pos != -1) {
alecguertin 15:14d4e7021125 90 timer.start();
alecguertin 15:14d4e7021125 91 pi.forward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 92 } else {
alecguertin 15:14d4e7021125 93 pi.cls();
alecguertin 15:14d4e7021125 94 pi.locate(0,0);
alecguertin 15:14d4e7021125 95 pi.printf("LP: %f",pos);
alecguertin 15:14d4e7021125 96 return 1;
alecguertin 15:14d4e7021125 97 }
alecguertin 15:14d4e7021125 98
alecguertin 15:14d4e7021125 99 wait(1);
lsaristo 12:1aa6b8a74136 100
lsaristo 12:1aa6b8a74136 101 do {
lsaristo 12:1aa6b8a74136 102 pos = pi.line_position();
lsaristo 13:070846d87d4a 103 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 104 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 105 pi.left_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 106 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 107 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 108 pi.right_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 109 } else {
alecguertin 15:14d4e7021125 110 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 111 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 112 }
alecguertin 15:14d4e7021125 113 pi.cls();
alecguertin 15:14d4e7021125 114 pi.locate(0,0);
alecguertin 15:14d4e7021125 115 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 116 } while(pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 117 pi.stop();
alecguertin 15:14d4e7021125 118 if(pos != -1) {
alecguertin 15:14d4e7021125 119 oled_1 = 1;
alecguertin 15:14d4e7021125 120 timer.stop();
alecguertin 15:14d4e7021125 121 pi.printf("T: %d", timer.read_ms());
alecguertin 15:14d4e7021125 122 } else {
alecguertin 15:14d4e7021125 123 pi.cls();
alecguertin 15:14d4e7021125 124 pi.locate(0,0);
alecguertin 15:14d4e7021125 125 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 126 return 1;
alecguertin 15:14d4e7021125 127 }
alecguertin 15:14d4e7021125 128 pi.right(TURN_SPEED);
alecguertin 15:14d4e7021125 129 wait(0.5);
alecguertin 15:14d4e7021125 130 pi.stop();
alecguertin 15:14d4e7021125 131 do {
alecguertin 15:14d4e7021125 132 pos = pi.line_position();
alecguertin 15:14d4e7021125 133 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 134 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 135 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 136 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 137 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 138 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 139 } else {
alecguertin 15:14d4e7021125 140 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 141 pi.left_motor(-DRIVE_SPEED);
lsaristo 13:070846d87d4a 142 }
lsaristo 12:1aa6b8a74136 143 pi.cls();
lsaristo 12:1aa6b8a74136 144 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 145 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 146 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 147 pi.stop();
alecguertin 15:14d4e7021125 148 if(pos != -1) {
alecguertin 15:14d4e7021125 149 oled_1 = 1;
alecguertin 15:14d4e7021125 150 timer.stop();
alecguertin 15:14d4e7021125 151 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 152 } else {
lsaristo 12:1aa6b8a74136 153 pi.cls();
lsaristo 12:1aa6b8a74136 154 pi.locate(0,0);
alecguertin 15:14d4e7021125 155 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 156 return 1;
lsaristo 12:1aa6b8a74136 157 }
John Wilkey 6:00b7198f0b51 158
lsaristo 12:1aa6b8a74136 159 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 160 do {
lsaristo 12:1aa6b8a74136 161 pos = pi.line_position();
lsaristo 13:070846d87d4a 162 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 163 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 164 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 165 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 166 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 167 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 168 } else {
lsaristo 13:070846d87d4a 169 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 170 }
lsaristo 12:1aa6b8a74136 171 pi.cls();
lsaristo 12:1aa6b8a74136 172 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 173 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 174 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 175 if(pos == 1) {
lsaristo 12:1aa6b8a74136 176 oled_1 = 1;
lsaristo 12:1aa6b8a74136 177 timer.stop();
lsaristo 12:1aa6b8a74136 178 pi.stop();
lsaristo 12:1aa6b8a74136 179 } else {
lsaristo 12:1aa6b8a74136 180 pi.stop();
lsaristo 12:1aa6b8a74136 181 pi.cls();
lsaristo 12:1aa6b8a74136 182 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 183 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 184 while(1);
lsaristo 12:1aa6b8a74136 185 }
lsaristo 14:41fa8b95a9ab 186 // If we got here, calibration is complete.
lsaristo 12:1aa6b8a74136 187
John Wilkey 6:00b7198f0b51 188 //
lsaristo 9:3a0433c391cb 189 // Main program loop.
lsaristo 12:1aa6b8a74136 190 // robot_loop();
alecguertin 17:c72c092fcdf7 191 size_t bytes_read = 0;
alecguertin 17:c72c092fcdf7 192 int err, x, y, last_x, last_y;
alecguertin 17:c72c092fcdf7 193 int offset = 0;
alecguertin 17:c72c092fcdf7 194 char *cur, *next;
alecguertin 15:14d4e7021125 195 ps_file = fopen("/local/test.ps", "r");
alecguertin 17:c72c092fcdf7 196 /* PS parsing loop. */
alecguertin 17:c72c092fcdf7 197 while (1) {
alecguertin 17:c72c092fcdf7 198 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 199 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 17:c72c092fcdf7 200 if (bytes_read == 0) {
alecguertin 17:c72c092fcdf7 201 break;
alecguertin 17:c72c092fcdf7 202 }
alecguertin 17:c72c092fcdf7 203 cur = instbuf;
alecguertin 17:c72c092fcdf7 204 while (cur[0] != '\0') {
alecguertin 17:c72c092fcdf7 205 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 17:c72c092fcdf7 206 move(x, y, draw);
alecguertin 17:c72c092fcdf7 207 last_x = x;
alecguertin 17:c72c092fcdf7 208 last_y = y;
alecguertin 17:c72c092fcdf7 209 next = strchr(cur, '\n');
alecguertin 17:c72c092fcdf7 210 if (next == NULL) {
alecguertin 17:c72c092fcdf7 211 break;
alecguertin 17:c72c092fcdf7 212 }
alecguertin 17:c72c092fcdf7 213 cur = next+1;
alecguertin 17:c72c092fcdf7 214 }
alecguertin 17:c72c092fcdf7 215 offset = instbuf+instbuflen-cur;
alecguertin 17:c72c092fcdf7 216 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 217 }
lsaristo 9:3a0433c391cb 218 //
lsaristo 9:3a0433c391cb 219 // We should never reach this point!
alecguertin 15:14d4e7021125 220 return 0;
John Wilkey 5:01882c3de2dc 221 }
lsaristo 8:12d780f7443e 222
alecguertin 17:c72c092fcdf7 223 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 224 {
alecguertin 17:c72c092fcdf7 225 int matches;
alecguertin 17:c72c092fcdf7 226 char *srch;
alecguertin 17:c72c092fcdf7 227 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 228 if (matches != 2) {
alecguertin 17:c72c092fcdf7 229 return 0;
alecguertin 17:c72c092fcdf7 230 }
alecguertin 17:c72c092fcdf7 231 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 232 srch++;
alecguertin 17:c72c092fcdf7 233 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 234 srch++;
alecguertin 17:c72c092fcdf7 235 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 236 *draw = 0;
alecguertin 17:c72c092fcdf7 237 } else if (srch[0] == 'd') {
alecguertin 17:c72c092fcdf7 238 *draw = 1;
alecguertin 17:c72c092fcdf7 239 } else {
alecguertin 17:c72c092fcdf7 240 return 0;
alecguertin 17:c72c092fcdf7 241 }
alecguertin 17:c72c092fcdf7 242 return 1;
alecguertin 17:c72c092fcdf7 243 }
alecguertin 17:c72c092fcdf7 244
lsaristo 9:3a0433c391cb 245 int forward(int amt)
lsaristo 8:12d780f7443e 246 {
lsaristo 12:1aa6b8a74136 247 Timer t;
lsaristo 12:1aa6b8a74136 248 t.start();
lsaristo 10:94b068b2ce1d 249 oled_2 = 1;
lsaristo 9:3a0433c391cb 250 pi.locate(0,0);
lsaristo 9:3a0433c391cb 251 pi.printf("Fwd %d", amt);
lsaristo 9:3a0433c391cb 252 pi.forward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 253 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 254 t.stop();
lsaristo 10:94b068b2ce1d 255 oled_2 = 0;
lsaristo 12:1aa6b8a74136 256 pi.stop();
lsaristo 8:12d780f7443e 257 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 258 }
lsaristo 8:12d780f7443e 259
lsaristo 9:3a0433c391cb 260 int backward(int amt)
lsaristo 8:12d780f7443e 261 {
lsaristo 12:1aa6b8a74136 262 Timer t;
lsaristo 10:94b068b2ce1d 263 oled_3 = 1;
lsaristo 9:3a0433c391cb 264 pi.locate(0,0);
lsaristo 9:3a0433c391cb 265 pi.printf("Back %d", amt);
lsaristo 9:3a0433c391cb 266 pi.backward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 267 t.start();
lsaristo 12:1aa6b8a74136 268 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 269 t.stop();
lsaristo 10:94b068b2ce1d 270 oled_3 = 0;
lsaristo 12:1aa6b8a74136 271 pi.stop();
lsaristo 8:12d780f7443e 272 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 273 }
lsaristo 8:12d780f7443e 274
lsaristo 8:12d780f7443e 275 int right(float deg)
lsaristo 8:12d780f7443e 276 {
lsaristo 12:1aa6b8a74136 277 Timer t;
lsaristo 10:94b068b2ce1d 278 oled_4 = 1;
lsaristo 9:3a0433c391cb 279 pi.locate(0,0);
lsaristo 9:3a0433c391cb 280 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 281 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 282 t.start();
lsaristo 12:1aa6b8a74136 283 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 284 t.stop();
lsaristo 10:94b068b2ce1d 285 oled_4 = 0;
lsaristo 12:1aa6b8a74136 286 pi.stop();
lsaristo 8:12d780f7443e 287 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 288 }
lsaristo 8:12d780f7443e 289
lsaristo 8:12d780f7443e 290 int left(float deg)
lsaristo 8:12d780f7443e 291 {
lsaristo 12:1aa6b8a74136 292 Timer t;
lsaristo 10:94b068b2ce1d 293 oled_4 = 1;
lsaristo 10:94b068b2ce1d 294 oled_2 = 1;
lsaristo 9:3a0433c391cb 295 pi.locate(0,0);
lsaristo 9:3a0433c391cb 296 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 297 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 298 t.start();
lsaristo 12:1aa6b8a74136 299 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 300 t.stop();
lsaristo 10:94b068b2ce1d 301 oled_4 = 0;
lsaristo 10:94b068b2ce1d 302 oled_2 = 0;
lsaristo 12:1aa6b8a74136 303 pi.stop();
lsaristo 8:12d780f7443e 304 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 305 }
lsaristo 10:94b068b2ce1d 306
lsaristo 10:94b068b2ce1d 307 void pen_down()
lsaristo 10:94b068b2ce1d 308 {
lsaristo 10:94b068b2ce1d 309 oled_1 = 1;
lsaristo 10:94b068b2ce1d 310 }
lsaristo 10:94b068b2ce1d 311
lsaristo 10:94b068b2ce1d 312 void pen_up()
lsaristo 10:94b068b2ce1d 313 {
lsaristo 10:94b068b2ce1d 314 oled_1 = 0;
lsaristo 9:3a0433c391cb 315 }