EE149
Final Project files for mBed development.
Diff: main.c
- Revision:
- 39:cc8691700d2a
- Parent:
- 37:1d51cf101b03
- Child:
- 40:0199bad6c979
--- a/main.c Sat Dec 13 03:34:04 2014 +0000
+++ b/main.c Sat Dec 13 07:35:57 2014 +0000
@@ -26,194 +26,127 @@
/** @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];
-
- // ---------------------------------------------------------
- // First calibrate robot and figure out space dimensions.
- pi.cls();
- pi.locate(0,1);
- pi.printf("%f mV", pi.battery());
- pi.sensor_auto_calibrate();
- timerWait(.2);
+ Timer caltimer;
+ FILE *ps_file;
+ double dist;
+ float delta_a, angle, cal_time;
+ size_t bytes_read;
+ int corner, err, x, y, last_x, last_y, dim_x, dim_y, offset;
+ char instbuf[INST_BUF_SIZE];
+ char *cur, *next;
+ angle = 0;
+ last_x = 0;
+ last_y = 0;
+ bytes_read = 0;
+ offset = 0;
+ int init = 0;
- 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 {
- oled_1 = 1;
- while(1);
+ /* First calibrate robot and figure out space dimensions. */
+ robot_printf(0, "%f mV", pi.battery());
+
+ /* Calibrate robot reflective sensors to line. */
+ pi.sensor_auto_calibrate();
+ timerWait(0.2);
+
+ /* Find left bottom right corner of frame. */
+ corner = find_corner(1);
+ if (!corner) {
+ robot_printf(0, "no left\n");
+ while (1);
}
- left(180);
- Timer caltimer;
+ left(HALF_TURN);
+
+ /*
+ * Find bottom left corner of frame and
+ * and measure length of frame side.
+ */
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);
+ corner = find_corner(0);
+ if (!corner) {
+ robot_printf(0, "no right\n");
+ while (1);
+ }
caltimer.stop();
- cal_time = caltimer.read_ms()*2;
- pi.stop();
- if(pos != -1) {
- timer.stop();
- } else {
- oled_1 = 1;
- while(1);
- }
- right(183);
- timerWait(.2);
- /*int wiggle_count = 0;
-
- for( ; fabs(pos = pi.line_position()) > CLOSE_ENOUGH
- && wiggle_count < WIGGLE_MAX; wiggle_count++) {
- pi.right((pos < 0 ? -.6*.4*CAL_SPEED : .6*.4*CAL_SPEED));
- pi.cls();
- pi.locate(0,0);
- pi.printf("O: %f", pos);
- }*/
- pi.stop();
+ cal_time = caltimer.read_ms();
+
+ /* Back up into corner (coordinate x=0,y=0). */
+ right(HALF_TURN);
timerWait(0.2);
backward(400);
- //while(1);
-
- // ---------------------------------------------------------
- // DONE calibrating. Begin reading in coordinate
- // file and run robot.
- 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;
+
+ /* Open instruction file. */
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);
+ /* Read beginning of file into buffer. */
+ memset(instbuf, 0, INST_BUF_SIZE);
+ bytes_read = fread(instbuf, sizeof(char), INST_BUF_SIZE-1, ps_file);
if (bytes_read == 0) {
return 1;
}
+
+ /* Read first line of file - the dimensions of the frame. */
err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
if (err != 2) {
- pi.cls();
- pi.locate(0,0);
- pi.printf("sscanf1");
+ robot_printf(0, "sscanf1");
while(1);
}
cur = strchr(instbuf, '\n');
cur++;
- offset = instbuf+instbuflen-cur-1;
+ offset = instbuf+INST_BUF_SIZE-cur-1;
memcpy(instbuf, cur, offset);
-
-
- // ---------------------------------------------------------
- // File open and buffer setup. Begin reading instructions and
- // moving robot. Refill buffer after each instruction.
+ /*
+ * File open and buffer setup. Begin reading instructions and
+ * moving robot. Refill buffer after each instruction.
+ */
while (1) {
- memset(instbuf+offset, 0, instbuflen-offset);
- bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
-
+ memset(instbuf+offset, 0, INST_BUF_SIZE-offset);
+ bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file);
if (instbuf[0] == '\0') {
- pi.cls();
- pi.locate(0,0);
- pi.printf("%s", instbuf+1);
+ robot_printf(0, "%s", instbuf+1);
oled_3 = 0;
while(1);
}
cur = instbuf;
err = retrieve_inst(instbuf, &x, &y, &draw);
-
if (err == 0) {
- pi.cls();
- pi.locate(0,0);
- pi.printf("noinst");
+ robot_printf(0, "noinst");
return 1;
}
- delta_x = x-last_x;
- delta_y = y-last_y;
- if (delta_y == 0) {
- if (delta_x < 0) {
- theta = 180;
- } else {
- theta = 0;
+ if (0 && !draw && init) {
+ float pos;
+ right(angle+QUARTER_TURN);
+ find_line();
+ forward(100);
+ pi.left(CAL_SPEED);
+ pos = pi.line_position();
+ while (pos > CORRECTION_THRESHOLD || pos < 0) {
+ pos = pi.line_position();
}
- } else if (delta_x == 0) {
- if (delta_y < 0) {
- theta = -90;
- } else {
- theta = 90;
- }
- } else {
- /* 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;
+ pi.stop();
+ find_corner(1);
+ left(180);
+ find_corner(0);
+ right(HALF_TURN);
+ timerWait(0.5);
+ backward(400);
+ angle = 0;
+ last_x = 0;
+ last_y = 0;
}
- if (delta_a > 180) {
- delta_a -= 360;
- } else if (delta_a < -180) {
- delta_a = 360 + delta_a;
- }
+ init = 1;
+ /* Compute turn angle and turn. */
+ delta_a = compute_turn_angle(last_x, last_y, x, y, angle);
angle += delta_a;
- if (angle >= 360) {
- angle -= 360;
+ if (angle >= FULL_TURN) {
+ angle -= FULL_TURN;
}
- pi.cls();
- pi.locate(0,0);
- pi.printf("a:%f", delta_a);
- oled_3 = 0;
+ robot_printf(0, "a:%f", delta_a);
left(delta_a);
/* Put pen into position. */
@@ -224,44 +157,135 @@
}
/* 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.55*cal_time*(dist/(double)dim_x));
- forward(0.55*cal_time*(dist/(double)dim_x));
+ dist = fabs(distance(last_x, last_y, x, y));
+ robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x));
+ forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
last_x = x;
last_y = y;
+ oled_3 = 0;
+
+ /* Seek to find next instruction. */
next = strchr(cur, '\n');
if (next == NULL) {
- pi.cls();
- pi.locate(0,0);
- pi.printf("nonext");
+ robot_printf(0, "nonext");
break;
}
cur = next+1;
- offset = instbuf+instbuflen-cur-1;
+ offset = instbuf+INST_BUF_SIZE-cur-1;
memcpy(instbuf, cur, offset);
}
- pi.cls();
- pi.locate(0,0);
- pi.printf("Done");
+ robot_printf(0, "Done");
pi.stop();
oled_3 = 0;
while (1);
}
+float compute_turn_angle(int last_x, int last_y, int x, int y, float angle)
+{
+ int delta_x, delta_y;
+ float theta, delta_a;
+ delta_x = x-last_x;
+ delta_y = y-last_y;
+ if (delta_y == 0) {
+ if (delta_x < 0) {
+ theta = HALF_TURN;
+ } else {
+ theta = 0;
+ }
+ } else if (delta_x == 0) {
+ if (delta_y < 0) {
+ theta = -QUARTER_TURN;
+ } else {
+ theta = QUARTER_TURN;
+ }
+ } else {
+ theta = atan(((double) delta_y)/((double) delta_x));
+ theta *= RAD_TO_DEG;
+ }
+ if (delta_x < 0 && delta_y > 0) {
+ theta += HALF_TURN;
+ }
+ delta_a = theta-angle;
+
+ if (delta_x < 0 && delta_y < 0) {
+ delta_a += HALF_TURN;
+ }
+
+ if (delta_a > HALF_TURN) {
+ delta_a -= FULL_TURN;
+ } else if (delta_a < -HALF_TURN) {
+ delta_a = FULL_TURN + delta_a;
+ }
+ return delta_a;
+}
+
+void robot_printf(int line, const char *format, ...)
+{
+ char buf[18];
+ va_list args;
+ pi.cls();
+ pi.locate(0,line);
+ va_start(args, format);
+ vsprintf(buf, format, args);
+ pi.printf("%s", buf);
+ va_end(args);
+}
+
+float distance(int x1, int y1, int x2, int y2)
+{
+ return sqrt(pow((double) (x2-x1),2) + pow((double) (y2-y1), 2));
+}
+
+void find_line()
+{
+ float pos, pos1;
+ pi.forward(DRIVE_SPEED);
+ pos1 = pi.line_position();
+ pos = pos1;
+ while (fabs(pos1-pos) < 0.1 || pos == -1) {
+ pos = pi.line_position();
+ robot_printf(0, "%f\n", pos);
+ }
+ robot_printf(0, "f:%f\n", pos);
+ pi.stop();
+}
+
+int find_corner(int left)
+{
+ float pos;
+ float threshold = CORNER_THRESHOLD;
+ if (left) {
+ threshold *= -1;
+ }
+ do {
+ pos = pi.line_position();
+ if(pos > CORRECTION_THRESHOLD) {
+ pi.right_motor(CAL_SPEED);
+ pi.left_motor(CAL_SPEED-CORRECTION_SPEED);
+ } else if(pos < -CORRECTION_THRESHOLD) {
+ pi.left_motor(CAL_SPEED);
+ pi.right_motor(CAL_SPEED-CORRECTION_SPEED);
+ } else {
+ pi.right_motor(CAL_SPEED);
+ pi.left_motor(CAL_SPEED);
+ }
+ robot_printf(0, "Pos: %f", pos);
+ } while(pos != -1 && ((left && pos > threshold) || (!left && pos < threshold)));
+ pi.stop();
+ if(pos == -1) {
+ oled_1 = 1;
+ return 0;
+ }
+ return 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");
+ robot_printf(0, "nomatch");
return 0;
}
srch = strchr(buf, ' ');
@@ -273,9 +297,7 @@
} else if (srch[0] == 'l') {
*draw = 1;
} else {
- pi.cls();
- pi.locate(0,0);
- pi.printf("%.8s", srch);
+ robot_printf(0, "%.8s", srch);
return 0;
}
return 1;
@@ -286,8 +308,7 @@
Timer t;
float ms = 0;
float last_ms = 0;
- pi.locate(0,0);
- pi.printf("Fwd %d", amt);
+ robot_printf(0, "Fwd %d", amt);
t.start();
pi.left_motor(DRIVE_SPEED+.002);
pi.right_motor(DRIVE_SPEED);
@@ -315,15 +336,28 @@
pi.stop();
}
+void left(float deg)
+{
+ Timer t;
+ deg += DEGREE_CORRECTION;
+ if(deg < 0) {
+ return right(-1*deg);
+ }
+ float amt = (((float)deg)/FULL_TURN)*TIME_FACT*1000;
+ pi.left(TURN_SPEED);
+ t.start();
+ while(t.read_us() < amt);
+ pi.stop();
+}
+
void right(float deg)
{
- deg += 2;
+ Timer t;
+ deg += DEGREE_CORRECTION;
if(deg < 0) {
return left(-1*deg);
}
- deg = deg;
- Timer t;
- float amt = ((((float)deg)/360)*TIME_FACT*1000);
+ float amt = ((((float)deg)/FULL_TURN)*TIME_FACT*1000);
t.start();
pi.right(TURN_SPEED);
while(t.read_us() < amt);
@@ -335,17 +369,4 @@
Timer t;
t.start();
while(t.read_us() < 1000000*seconds);
-}
-
-void left(float deg)
-{
- if(deg < 0) {
- return right(-1*deg);
- }
- float amt = (((float)deg)/360)*TIME_FACT*1000;
- Timer t;
- pi.left(TURN_SPEED);
- t.start();
- while(t.read_us() < amt);
- pi.stop();
}
\ No newline at end of file