EE149
/
FinalProject
Final Project files for mBed development.
main.c
- Committer:
- lsaristo
- Date:
- 2014-12-08
- Revision:
- 29:459ff10d2a07
- Parent:
- 28:9976a94efa83
- Child:
- 30:3211e2962441
File content as of revision 29:459ff10d2a07:
/** * @file driver.c * @brief Primary control logic for robot. * * @author John Wilkey * @author Alec Guertin * @author Chester Chu */ #include "main.h" m3pi pi; /**< m3pi Object for all control operations. */ Timer timer; /**< Timer used to wait in timerWait. */ DigitalIn start_button(p21); /**< Pi start button input on mBed pin 32 */ int draw; /**< Boolean for drawing/moving. */ /* 4 mBed onboard LEDs */ DigitalOut oled_1(LED1), oled_2(LED2), oled_3(LED3), oled_4(LED4); /* 8 LEDs driven from mBed digital outs. */ DigitalOut pin13(p13), pin14(p14), pin15(p15), pin16(p16), pin17(p17), pin18(p18), pin19(p19), pin20(p20); /* Local File System */ LocalFileSystem local("local"); /** @brief Entry point. Main loop. */ int main() { FILE *ps_file; float cal_time; float pos = 0; float over_thresh = 0.05; float correction = 0.2*DRIVE_SPEED; int instbuflen = 250; char instbuf[instbuflen]; start_button.mode(PullUp); pi.cls(); pi.locate(0,0); pi.printf("Ready"); pi.locate(0,1); pi.printf("%f mV", pi.battery()); wait(.5); pi.sensor_auto_calibrate(); wait(1); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(CAL_SPEED); pi.left_motor(CAL_SPEED-correction); } else if(pos < -over_thresh) { pi.left_motor(CAL_SPEED); pi.right_motor(CAL_SPEED-correction); } else { pi.right_motor(CAL_SPEED); pi.left_motor(CAL_SPEED); } pi.cls(); pi.locate(0,0); pi.printf("Pos: %f", pos); } while(pos != -1 && pos > -0.3); pi.stop(); if(pos != -1) { timer.stop(); } else { return 1; } right(180); Timer caltimer; caltimer.start(); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(CAL_SPEED); pi.left_motor(CAL_SPEED-correction); } else if(pos < -over_thresh) { pi.left_motor(CAL_SPEED); pi.right_motor(CAL_SPEED-correction); } else { pi.right_motor(CAL_SPEED); pi.left_motor(CAL_SPEED); } pi.cls(); pi.locate(0,0); pi.printf("Pos: %f", pos); } while(pos != -1 && pos <= 0.3); caltimer.stop(); cal_time = caltimer.read_ms(); pi.stop(); if(pos != -1) { timer.stop(); } else { return 1; } right(180); timerWait(.5); while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) { pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED)); pi.cls(); pi.locate(0,0); pi.printf("O: %f", pos); } pi.stop(); timerWait(2); backward(500); timerWait(.5); while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) { pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED)); pi.cls(); pi.locate(0,0); pi.printf("O: %f", pos); } pi.stop(); timerWait(2); timerWait(1); // Main program loop. size_t bytes_read = 0; int err, x, y, last_x, last_y, delta_x, delta_y; float 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"); if (ps_file == NULL) { return 1; } /* PS parsing loop. */ memset(instbuf, 0, instbuflen); bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file); if (bytes_read == 0) { return 1; } err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y); if (err != 2) { pi.cls(); pi.locate(0,0); pi.printf("sscanf1"); return 1; } cur = strchr(instbuf, '\n'); cur++; offset = instbuf+instbuflen-cur-1; memcpy(instbuf, cur, offset); while (1) { timerWait(1); memset(instbuf+offset, 0, instbuflen-offset); bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file); if (instbuf[0] == '\0') { pi.cls(); pi.locate(0,0); pi.printf("%s", instbuf+1); while(1); } cur = instbuf; err = retrieve_inst(instbuf, &x, &y, &draw); if (err == 0) { pi.cls(); pi.locate(0,0); pi.printf("noinst"); return 1; } delta_x = x-last_x; delta_y = y-last_y; /* Compute turn angle and turn. */ theta = atan(((double) delta_y)/((double) delta_x)); theta *= 57.29; if (delta_x < 0 && delta_y > 0) { theta += 180; } delta_a = theta-angle; if (delta_x < 0 && delta_y < 0) { delta_a += 180; } if (delta_a > 180) { delta_a -= 360; } angle += delta_a; if (angle >= 360) { angle -= 360; } //pi.cls(); //pi.locate(0,0); //pi.printf("a:%f", delta_a); //timerWait(1.5); left(delta_a); /* Put pen into position. */ if (draw) { oled_3 = 1; } else { oled_3 = 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.cls(); //pi.locate(0,0); //pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x)); //timerWait(1.5); forward(0.4*cal_time*(dist/(double)dim_x)); last_x = x; last_y = y; next = strchr(cur, '\n'); if (next == NULL) { pi.cls(); pi.locate(0,0); pi.printf("nonext"); break; } cur = next+1; offset = instbuf+instbuflen-cur-1; memcpy(instbuf, cur, offset); } pi.cls(); pi.locate(0,0); pi.printf("Done"); while (1); } 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) { pi.cls(); pi.locate(0,0); pi.printf("nomatch"); return 0; } srch = strchr(buf, ' '); srch++; srch = strchr(srch, ' '); srch++; if (srch[0] == 'm') { *draw = 0; } else if (srch[0] == 'l') { *draw = 1; } else { pi.cls(); pi.locate(0,0); pi.printf("%.8s", srch); return 0; } return 1; } void forward(int amt) { Timer t; pi.locate(0,0); pi.printf("Fwd %d", amt); pi.left_motor(DRIVE_SPEED+.0023); pi.right_motor(DRIVE_SPEED); t.start(); while(t.read_ms() < amt); pi.left_motor(.7*DRIVE_SPEED+.0023); pi.right_motor(.7*DRIVE_SPEED); t.stop(); pi.stop(); } void backward(int amt) { Timer t; t.start(); pi.backward(.5*DRIVE_SPEED); while(t.read_ms() < (amt-500)); pi.backward(.5*DRIVE_SPEED); while(t.read_ms() < amt); t.stop(); pi.stop(); } void right(float deg) { if(deg < 0) { return left(-1*deg); } Timer t; pi.right(TURN_SPEED); t.start(); while(t.read_ms() < ((float)(deg/360)*TIME_FACT)); t.stop(); pi.stop(); } void timerWait(float seconds) { Timer t; t.start(); while(t.read_us() < 1000000*seconds); t.stop(); } void left(float deg) { if(deg < 0) { return right(-1*deg); } Timer t; pi.left(TURN_SPEED); t.start(); while(t.read_ms() < ((float)(deg/360)*TIME_FACT)); t.stop(); pi.stop(); } void pen_down() { oled_1 = 1; } void pen_up() { oled_1 = 0; }