EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 21:0c80a5d89ea3
- Parent:
- 20:76718145b403
- Child:
- 22:46b9d9b2e35c
diff -r 76718145b403 -r 0c80a5d89ea3 main.c --- a/main.c Tue Dec 02 03:42:32 2014 +0000 +++ b/main.c Sat Dec 06 22:04:53 2014 +0000 @@ -56,28 +56,57 @@ // Basic setup information start_button.mode(PullUp); + start: + pi.cls(); + pi.locate(0,0); + pi.printf("PiCO"); + pi.locate(0,1); + pi.printf("%f mV", pi.battery()); + wait(.5); + /* while(start_button) { + oled_2 = 1; + wait(.5); + pi.locate(0,0); + pi.printf("Ready"); + oled_2 = 0; + } /**/ + pi.cls(); + pi.locate(0,0); + // pi.printf("GO!"); + wait(.5); + + /* + while(1) { + if(!start_button) { + pi.stop(); + goto start; + } else { + goto calibrate; + } + } + /**/ + // // Drawing environment calibration. + int cal_count = 1; 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); + pi.left_motor(-CAL_SPEED); + pi.right_motor(-CAL_SPEED+correction); } else if(pos < -over_thresh) { - pi.left_motor(-DRIVE_SPEED); - pi.right_motor(-DRIVE_SPEED+correction); + pi.right_motor(-CAL_SPEED); + pi.left_motor(-CAL_SPEED+correction); } else { - pi.right_motor(-DRIVE_SPEED); - pi.left_motor(-DRIVE_SPEED); + pi.backward(CAL_SPEED); } pi.cls(); pi.locate(0,0); @@ -85,28 +114,62 @@ } while (pos != -1 && pos <= 0.3); pi.stop(); wait(1); + if (pos != -1) { timer.start(); - pi.forward(DRIVE_SPEED); + pi.forward(CAL_SPEED); } else { pi.cls(); pi.locate(0,0); pi.printf("LP: %f",pos); + forward(1000); + goto calibrate; return 1; } - + /**/ + + // 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) { + 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; + } + right(180); wait(1); do { pos = pi.line_position(); if(pos > over_thresh) { - pi.right_motor(DRIVE_SPEED); - pi.left_motor(DRIVE_SPEED-correction); + pi.right_motor(CAL_SPEED); + pi.left_motor(CAL_SPEED-correction); } else if(pos < -over_thresh) { - pi.left_motor(DRIVE_SPEED); - pi.right_motor(DRIVE_SPEED-correction); + pi.left_motor(CAL_SPEED); + pi.right_motor(CAL_SPEED-correction); } else { - pi.right_motor(DRIVE_SPEED); - pi.left_motor(DRIVE_SPEED); + pi.right_motor(CAL_SPEED); + pi.left_motor(CAL_SPEED); } pi.cls(); pi.locate(0,0); @@ -123,9 +186,24 @@ pi.printf("lP:%f", pos); return 1; } - pi.right(TURN_SPEED); - wait(0.5); - pi.stop(); + right(180); + timerWait(.07); + pos = pi.line_position(); + while(fabs(pos) > CLOSE_ENOUGH) { + pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED)); + timerWait(.08); + pi.cls(); + pi.locate(0,0); + pi.printf("O: %f", pos); + pi.stop(); + pos = pi.line_position(); + timerWait(.2); + } + timerWait(2); + + // + // Pivot 180 degrees to go to the starting position. + /* do { pos = pi.line_position(); if(pos > over_thresh) { @@ -135,8 +213,7 @@ pi.left_motor(-DRIVE_SPEED); pi.right_motor(-DRIVE_SPEED+correction); } else { - pi.right_motor(-DRIVE_SPEED); - pi.left_motor(-DRIVE_SPEED); + pi.backward(DRIVE_SPEED); } pi.cls(); pi.locate(0,0); @@ -156,6 +233,7 @@ pi.printf("lP:%f", pos); return 1; } + /**/ /* while(pi.line_position() == 1); do { @@ -271,10 +349,19 @@ if (dist < 0) { dist *= -1; } - // forward(cal_time*(dist/dim_x)); - forward(5000); - // wait(cal_time*(dist/dim_x)); - // pi.stop(); + timerWait(.2); + forward(1500); + right(90); + timerWait(.2); + forward(1500); + right(90); + timerWait(.2); + forward(1500); + right(90); + timerWait(.2); + forward(1500); + right(90); + timerWait(.2); last_x = x; last_y = y; @@ -338,7 +425,11 @@ oled_2 = 1; pi.locate(0,0); pi.printf("Fwd %d", amt); - pi.forward(DRIVE_SPEED); + pi.left_motor(DRIVE_SPEED+.0023); + pi.right_motor(DRIVE_SPEED); + while(t.read_ms() < (amt-500)); + pi.left_motor(.7*DRIVE_SPEED+.0023); + pi.right_motor(.7*DRIVE_SPEED); while(t.read_ms() < amt); t.stop(); oled_2 = 0; @@ -369,13 +460,21 @@ pi.printf("Right %f", deg); pi.right(TURN_SPEED); t.start(); - while(t.read_ms() < (deg/360)*1000); + while(t.read_ms() < (deg/360)*TIME_FACT); t.stop(); oled_4 = 0; pi.stop(); return EXIT_SUCCESS; } +void timerWait(float seconds) +{ + Timer t; + t.start(); + while(t.read_us() < 1000000*seconds); + t.stop(); +} + int left(float deg) { Timer t; @@ -385,7 +484,7 @@ pi.printf("Left %f", deg); pi.left(TURN_SPEED); t.start(); - while(t.read_ms() < (deg/360)*1000); + while(t.read_ms() < (deg/360)*TIME_FACT); t.stop(); oled_4 = 0; oled_2 = 0;