EE149
/
FinalProject
Final Project files for mBed development.
main.c
- Committer:
- alecguertin
- Date:
- 2014-12-01
- Revision:
- 19:47759cf4f9b9
- Parent:
- 18:eab7b0e89398
- Child:
- 20:76718145b403
File content as of revision 19:47759cf4f9b9:
/** * @file driver.c * @brief Basic driver program for our robot's controller logic. * * Maybe add lots of stuff here or maybe split it off into * multiple subfiles? * * @author John Wilkey - aw hells no! you ain't takin' credit for all this! */ #include "main.h" #include "control.h" /** * These are global data Used externally in all other files */ m3pi pi; Timer timer; // // Digital inputs to the mBed DigitalIn start_button(p21); // // Digital outputs from the mBed. Note that by default these are // used to drive the 8 LED's on the top board. DigitalOut pin15(p15); DigitalOut pin16(p16); DigitalOut pin17(p17); DigitalOut pin18(p18); DigitalOut pin19(p19); DigitalOut pin20(p20); // // mBed onboard LEDs DigitalOut oled_1(LED1); DigitalOut oled_2(LED2); DigitalOut oled_3(LED3); DigitalOut oled_4(LED4); /* Local File System */ LocalFileSystem local("local"); /* Boolean for drawing/moving. */ int draw; /** * @brief Entry point. Main loop. */ int main() { FILE *ps_file; int instbuflen = 250; char instbuf[instbuflen]; // // Basic setup information start_button.mode(PullUp); // // Drawing environment calibration. pi.sensor_auto_calibrate(); //pi.backward(DRIVE_SPEED); float pos = 0; float over_thresh = 0.05; float correction = 0.2*DRIVE_SPEED; float cal_time; wait(1); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(-DRIVE_SPEED); pi.left_motor(-DRIVE_SPEED+correction); } else if(pos < -over_thresh) { pi.left_motor(-DRIVE_SPEED); pi.right_motor(-DRIVE_SPEED+correction); } else { pi.right_motor(-DRIVE_SPEED); pi.left_motor(-DRIVE_SPEED); } /* pi.cls(); pi.locate(0,0); pi.printf("P: %f", pos); */ } while (pos != -1 && pos <= 0.3); pi.stop(); wait(1); if (pos != -1) { timer.start(); pi.forward(DRIVE_SPEED); } else { pi.cls(); pi.locate(0,0); pi.printf("LP: %f",pos); return 1; } wait(1); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(DRIVE_SPEED); pi.left_motor(DRIVE_SPEED-correction); } else if(pos < -over_thresh) { pi.left_motor(DRIVE_SPEED); pi.right_motor(DRIVE_SPEED-correction); } else { pi.right_motor(DRIVE_SPEED); pi.left_motor(DRIVE_SPEED); } pi.cls(); pi.locate(0,0); pi.printf("Pos: %f", pos); } while(pos != -1 && pos <= 0.3); pi.stop(); if(pos != -1) { oled_1 = 1; timer.stop(); pi.printf("T: %d", timer.read_ms()); } else { pi.cls(); pi.locate(0,0); pi.printf("lP:%f", pos); return 1; } pi.right(TURN_SPEED); wait(0.5); pi.stop(); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(-DRIVE_SPEED); pi.left_motor(-DRIVE_SPEED+correction); } else if(pos < -over_thresh) { pi.left_motor(-DRIVE_SPEED); pi.right_motor(-DRIVE_SPEED+correction); } else { pi.right_motor(-DRIVE_SPEED); pi.left_motor(-DRIVE_SPEED); } pi.cls(); pi.locate(0,0); pi.printf("P: %f", pos); } while (pos != -1 && pos > -0.3); pi.stop(); if(pos != -1) { oled_1 = 1; timer.stop(); cal_time = timer.read(); pi.printf("T: %d", timer.read_ms()); } else { pi.cls(); pi.locate(0,0); pi.printf("lP:%f", pos); return 1; } while(pi.line_position() == 1); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(DRIVE_SPEED); pi.left_motor(DRIVE_SPEED - correction); } else if(pos < -over_thresh) { pi.left_motor(DRIVE_SPEED); pi.right_motor(DRIVE_SPEED - correction); } else { pi.forward(DRIVE_SPEED); } pi.cls(); pi.locate(0,0); pi.printf("Pos: %f", pos); } while(pos != -1 && pos != 1); if(pos == 1) { oled_1 = 1; timer.stop(); pi.stop(); } else { pi.stop(); pi.cls(); pi.locate(0,0); pi.printf("LP:%f", pos); while(1); } // If we got here, calibration is complete. // // Main program loop. // robot_loop(); size_t bytes_read = 0; int err, x, y, last_x, last_y, delta_x, delta_y, delta_a; int dim_x, dim_y; int offset = 0; char *cur, *next; float angle; double dist, theta; angle = 0; theta = 0; last_x = 0; last_y = 0; ps_file = fopen("/local/test.ps", "r"); /* PS parsing loop. */ memset(instbuf, 0, instbuflen); bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file); err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y); if (err != 2) { return 1; } cur = strchr(instbuf, '\n'); while (1) { memset(instbuf+offset, 0, instbuflen-offset); bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file); if (bytes_read == 0) { break; } cur = instbuf; while (cur[0] != '\0') { err = retrieve_inst(instbuf, &x, &y, &draw); if (err == 0) { return 1; } delta_x = x-last_x; delta_y = y-last_y; /* Compute turn angle and turn. */ theta = tan((double) delta_x/delta_y); theta *= 57.2957795; delta_a = theta-angle; if (delta_a > 0) { pi.left(TURN_SPEED); } else { pi.right(TURN_SPEED); } wait(0.5*(delta_a/360)); /* Put pen into position. */ if (draw) { oled_1 = 1; } else { oled_1 = 0; } /* Compute drive time and move forward. */ dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2)); if (dist < 0) { dist *= -1; } pi.forward(DRIVE_SPEED); wait(cal_time*(dist/dim_x)); last_x = x; last_y = y; next = strchr(cur, '\n'); if (next == NULL) { break; } cur = next+1; } offset = instbuf+instbuflen-cur; memcpy(instbuf, cur, offset); } // // We should never reach this point! // // Yes, we should... return 0; } int retrieve_inst(char *buf, int *x, int *y, int *draw) { int matches; char *srch; matches = sscanf(buf, "%d %d", x, y); if (matches != 2) { return 0; } srch = strchr(buf, ' '); srch++; srch = strchr(buf, ' '); srch++; if (srch[0] == 'm') { *draw = 0; } else if (srch[0] == 'd') { *draw = 1; } else { return 0; } return 1; } int forward(int amt) { Timer t; t.start(); oled_2 = 1; pi.locate(0,0); pi.printf("Fwd %d", amt); pi.forward(DRIVE_SPEED); while(t.read_ms() < amt*DRIVE_RATE*1000); t.stop(); oled_2 = 0; pi.stop(); return EXIT_SUCCESS; } int backward(int amt) { Timer t; oled_3 = 1; pi.locate(0,0); pi.printf("Back %d", amt); pi.backward(DRIVE_SPEED); t.start(); while(t.read_ms() < amt*DRIVE_RATE*1000); t.stop(); oled_3 = 0; pi.stop(); return EXIT_SUCCESS; } int right(float deg) { Timer t; oled_4 = 1; pi.locate(0,0); pi.printf("Right %f", deg); pi.right(TURN_SPEED); t.start(); while(t.read_ms() < (deg/360)*1000); t.stop(); oled_4 = 0; pi.stop(); return EXIT_SUCCESS; } int left(float deg) { Timer t; oled_4 = 1; oled_2 = 1; pi.locate(0,0); pi.printf("Left %f", deg); pi.left(TURN_SPEED); t.start(); while(t.read_ms() < (deg/360)*1000); t.stop(); oled_4 = 0; oled_2 = 0; pi.stop(); return EXIT_SUCCESS; } void pen_down() { oled_1 = 1; } void pen_up() { oled_1 = 0; }