EE149
Final Project files for mBed development.
Diff: main.c
- Revision:
- 22:46b9d9b2e35c
- Parent:
- 21:0c80a5d89ea3
- Child:
- 23:e4616259a7f0
--- a/main.c Sat Dec 06 22:04:53 2014 +0000
+++ b/main.c Sun Dec 07 04:39:33 2014 +0000
@@ -158,7 +158,8 @@
return 1;
}
right(180);
- wait(1);
+ Timer caltimer;
+ caltimer.start();
do {
pos = pi.line_position();
if(pos > over_thresh) {
@@ -175,6 +176,8 @@
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) {
oled_1 = 1;
@@ -187,7 +190,52 @@
return 1;
}
right(180);
- timerWait(.07);
+ timerWait(.5);
+ while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
+ pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
+ // timerWait(.08);
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("O: %f", pos);
+ // pi.stop();
+ // timerWait(.2);
+ }
+ pi.stop();
+ timerWait(2);
+ /*
+ pos = pi.line_position();
+ do {
+ pi.backward(CAL_SPEED);
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("p:%f", pos);
+ pos = pi.line_position();
+ } while (pos != -1 && pos >= -0.25);
+ /**/
+ backward(500);
+ timerWait(.5);
+ while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
+ pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
+ // timerWait(.08);
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("O: %f", pos);
+ // pi.stop();
+ // timerWait(.2);
+ }
+ pi.stop();
+ timerWait(2);
+ /*
+ pos = pi.line_position();
+ do {
+ pi.forward(CAL_SPEED);
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("p:%f", pos);
+ pos = pi.line_position();
+ } while (pos <= -0.07);
+ timerWait(0.25);
+ pi.stop();
pos = pi.line_position();
while(fabs(pos) > CLOSE_ENOUGH) {
pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
@@ -199,8 +247,8 @@
pos = pi.line_position();
timerWait(.2);
}
- timerWait(2);
-
+ /**/
+ timerWait(1);
//
// Pivot 180 degrees to go to the starting position.
/*
@@ -269,7 +317,8 @@
// 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 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;
@@ -289,6 +338,9 @@
/* PS parsing loop. */
memset(instbuf, 0, instbuflen);
bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
+ if (bytes_read == 0) {
+ return 1;
+ }
pi.cls();
pi.locate(0,0);
pi.printf("%.7s", instbuf);
@@ -301,7 +353,7 @@
}
cur = strchr(instbuf, '\n');
cur++;
- offset = instbuf+instbuflen-cur;
+ offset = instbuf+instbuflen-cur-1;
memcpy(instbuf, cur, offset);
while (1) {
memset(instbuf+offset, 0, instbuflen-offset);
@@ -310,75 +362,68 @@
pi.cls();
pi.locate(0,0);
pi.printf("bytes0");
- break;
+ timerWait(2);
}
cur = instbuf;
- while (cur[0] != '\0') {
- 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 = tan((double) delta_x/delta_y);
- theta *= 57.2957795;
- delta_a = theta-angle;
+ err = retrieve_inst(instbuf, &x, &y, &draw);
+ if (err == 0) {
pi.cls();
pi.locate(0,0);
- pi.printf("%d", delta_a);
- if (delta_a > 0) {
- left(delta_a);
- } else {
- right(delta_a);
- }
-
- /* 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;
- }
- 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);
+ pi.printf("noinst");
+ return 1;
+ }
+ delta_x = x-last_x;
+ delta_y = y-last_y;
+
+ /* Compute turn angle and turn. */
+ theta = atan(((double) delta_x)/((double) delta_y));
+ theta *= 57.29;
+ delta_a = theta-angle;
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("a:%f", delta_a);
+ timerWait(2);
+ if (delta_a > 0) {
+ left(delta_a);
+ }
- 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;
+ /* 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.cls();
+ pi.locate(0,0);
+ pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
+ timerWait(2);
+ 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("waiting!");
+ pi.printf("nonext");
+ break;
}
- offset = instbuf+instbuflen-cur;
+ cur = next+1;
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("waiting!");
+ offset = instbuf+instbuflen-cur-1;
memcpy(instbuf, cur, offset);
+ pi.cls();
+ pi.locate(0,0);
+ pi.printf("%.08s", instbuf);
+ timerWait(2);
}
//
// We should never reach this point!
@@ -430,7 +475,6 @@
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;
pi.stop();
@@ -443,8 +487,10 @@
oled_3 = 1;
pi.locate(0,0);
pi.printf("Back %d", amt);
- pi.backward(DRIVE_SPEED);
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();
oled_3 = 0;
@@ -460,7 +506,7 @@
pi.printf("Right %f", deg);
pi.right(TURN_SPEED);
t.start();
- while(t.read_ms() < (deg/360)*TIME_FACT);
+ while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
t.stop();
oled_4 = 0;
pi.stop();
@@ -484,7 +530,7 @@
pi.printf("Left %f", deg);
pi.left(TURN_SPEED);
t.start();
- while(t.read_ms() < (deg/360)*TIME_FACT);
+ while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
t.stop();
oled_4 = 0;
oled_2 = 0;