EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 19:47759cf4f9b9
- Parent:
- 18:eab7b0e89398
- Child:
- 20:76718145b403
--- a/main.c Mon Dec 01 01:02:42 2014 +0000 +++ b/main.c Mon Dec 01 20:16:23 2014 +0000 @@ -5,7 +5,7 @@ * Maybe add lots of stuff here or maybe split it off into * multiple subfiles? * - * @author John Wilkey + * @author John Wilkey - aw hells no! you ain't takin' credit for all this! */ #include "main.h" #include "control.h" @@ -63,7 +63,8 @@ float pos = 0; float over_thresh = 0.05; float correction = 0.2*DRIVE_SPEED; - + float cal_time; + wait(1); do { @@ -148,6 +149,7 @@ if(pos != -1) { oled_1 = 1; timer.stop(); + cal_time = timer.read(); pi.printf("T: %d", timer.read_ms()); } else { pi.cls(); @@ -175,7 +177,7 @@ if(pos == 1) { oled_1 = 1; timer.stop(); - pi.stop(); + pi.stop(); } else { pi.stop(); pi.cls(); @@ -189,14 +191,25 @@ // Main program loop. // robot_loop(); size_t bytes_read = 0; - int err, x, y, last_x, last_y, delta_x, delta_y; + int err, x, y, last_x, last_y, delta_x, delta_y, delta_a; + int dim_x, dim_y; int offset = 0; char *cur, *next; - float angle, theta; + float angle; + double dist, theta; angle = 0; theta = 0; + last_x = 0; + last_y = 0; ps_file = fopen("/local/test.ps", "r"); /* PS parsing loop. */ + memset(instbuf, 0, instbuflen); + bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file); + err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y); + if (err != 2) { + return 1; + } + cur = strchr(instbuf, '\n'); while (1) { memset(instbuf+offset, 0, instbuflen-offset); bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file); @@ -206,11 +219,37 @@ cur = instbuf; while (cur[0] != '\0') { err = retrieve_inst(instbuf, &x, &y, &draw); + if (err == 0) { + return 1; + } delta_x = x-last_x; delta_y = y-last_y; - theta = tan(delta_x/delta_y); + + /* Compute turn angle and turn. */ + theta = tan((double) delta_x/delta_y); theta *= 57.2957795; delta_a = theta-angle; + if (delta_a > 0) { + pi.left(TURN_SPEED); + } else { + pi.right(TURN_SPEED); + } + wait(0.5*(delta_a/360)); + + /* 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.forward(DRIVE_SPEED); + wait(cal_time*(dist/dim_x)); + last_x = x; last_y = y; next = strchr(cur, '\n');