EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 39:cc8691700d2a
- Parent:
- 37:1d51cf101b03
- Child:
- 40:0199bad6c979
diff -r 69c7e86244e4 -r cc8691700d2a main.c --- 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