EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 20:76718145b403
- Parent:
- 19:47759cf4f9b9
- Child:
- 21:0c80a5d89ea3
--- a/main.c Mon Dec 01 20:16:23 2014 +0000 +++ b/main.c Tue Dec 02 03:42:32 2014 +0000 @@ -79,11 +79,9 @@ pi.right_motor(-DRIVE_SPEED); pi.left_motor(-DRIVE_SPEED); } - /* pi.cls(); pi.locate(0,0); pi.printf("P: %f", pos); - */ } while (pos != -1 && pos <= 0.3); pi.stop(); wait(1); @@ -98,7 +96,6 @@ } wait(1); - do { pos = pi.line_position(); if(pos > over_thresh) { @@ -150,6 +147,8 @@ oled_1 = 1; timer.stop(); cal_time = timer.read(); + pi.cls(); + pi.locate(0,0); pi.printf("T: %d", timer.read_ms()); } else { pi.cls(); @@ -157,7 +156,7 @@ pi.printf("lP:%f", pos); return 1; } - + /* while(pi.line_position() == 1); do { pos = pi.line_position(); @@ -185,6 +184,7 @@ pi.printf("LP:%f", pos); while(1); } + */ // If we got here, calibration is complete. // @@ -202,24 +202,45 @@ last_x = 0; last_y = 0; ps_file = fopen("/local/test.ps", "r"); + if (ps_file == NULL) { + return 1; + } + pi.cls(); + pi.locate(0,0); + pi.printf("open"); /* PS parsing loop. */ memset(instbuf, 0, instbuflen); - bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file); + bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file); + pi.cls(); + pi.locate(0,0); + pi.printf("%.7s", instbuf); err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y); if (err != 2) { + pi.cls(); + pi.locate(0,0); + pi.printf("sscanf1"); return 1; } cur = strchr(instbuf, '\n'); + cur++; + offset = instbuf+instbuflen-cur; + memcpy(instbuf, cur, offset); while (1) { memset(instbuf+offset, 0, instbuflen-offset); bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file); if (bytes_read == 0) { + pi.cls(); + pi.locate(0,0); + pi.printf("bytes0"); break; } 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; @@ -229,34 +250,45 @@ theta = tan((double) delta_x/delta_y); theta *= 57.2957795; delta_a = theta-angle; + pi.cls(); + pi.locate(0,0); + pi.printf("%d", delta_a); if (delta_a > 0) { - pi.left(TURN_SPEED); + left(delta_a); } else { - pi.right(TURN_SPEED); + right(delta_a); } - 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)); + // forward(cal_time*(dist/dim_x)); + forward(5000); + // wait(cal_time*(dist/dim_x)); + // pi.stop(); 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; + pi.cls(); + pi.locate(0,0); + pi.printf("waiting!"); } offset = instbuf+instbuflen-cur; memcpy(instbuf, cur, offset); @@ -265,6 +297,9 @@ // We should never reach this point! // // Yes, we should... + pi.cls(); + pi.locate(0,0); + pi.printf("done"); return 0; } @@ -274,17 +309,23 @@ char *srch; matches = sscanf(buf, "%d %d", x, y); if (matches != 2) { + pi.cls(); + pi.locate(0,0); + pi.printf("nomatch"); return 0; } srch = strchr(buf, ' '); srch++; - srch = strchr(buf, ' '); + srch = strchr(srch, ' '); srch++; if (srch[0] == 'm') { *draw = 0; - } else if (srch[0] == 'd') { + } else if (srch[0] == 'l') { *draw = 1; } else { + pi.cls(); + pi.locate(0,0); + pi.printf("%.8s", srch); return 0; } return 1; @@ -298,7 +339,7 @@ pi.locate(0,0); pi.printf("Fwd %d", amt); pi.forward(DRIVE_SPEED); - while(t.read_ms() < amt*DRIVE_RATE*1000); + while(t.read_ms() < amt); t.stop(); oled_2 = 0; pi.stop(); @@ -313,7 +354,7 @@ pi.printf("Back %d", amt); pi.backward(DRIVE_SPEED); t.start(); - while(t.read_ms() < amt*DRIVE_RATE*1000); + while(t.read_ms() < amt); t.stop(); oled_3 = 0; pi.stop();