EE149
Final Project files for mBed development.
Diff: main.c
- Revision:
- 21:0c80a5d89ea3
- Parent:
- 20:76718145b403
- Child:
- 22:46b9d9b2e35c
--- 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;