EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 41:1b6d8664d7a7
- Parent:
- 40:0199bad6c979
- Child:
- 42:727612987d77
--- a/main.c Wed Dec 17 05:13:37 2014 +0000 +++ b/main.c Thu Dec 18 03:31:58 2014 +0000 @@ -23,23 +23,36 @@ /* Local File System */ LocalFileSystem local("local"); +enum RobotState { + ERROR, + PARSE, + TURN, + MOVE, + DRAW, +}; + /** @brief Entry point. Main loop. */ int main() { Timer caltimer; FILE *ps_file; + RobotState state, last_state; 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; + int corner, err, init, not_done, x, y, last_x, last_y, dim_x, dim_y, offset; char instbuf[INST_BUF_SIZE]; char *cur, *next; angle = 0; + x = 0; + y = 0; last_x = 0; last_y = 0; bytes_read = 0; offset = 0; - int init = 0; + init = 0; + not_done = 1; + caltimer = Timer(); /* First calibrate robot and figure out space dimensions. */ robot_printf(0, "%f mV", pi.battery()); @@ -101,6 +114,84 @@ * File open and buffer setup. Begin reading instructions and * moving robot. Refill buffer after each instruction. */ + state = PARSE; + last_state = PARSE; + while (not_done) { + switch (state) { + case PARSE: + if (!init) { + next = strchr(cur, '\n'); + if (next == NULL) { + last_state = state; + state = ERROR; + break; + } + cur = next+1; + offset = instbuf+INST_BUF_SIZE-cur-1; + memcpy(instbuf, cur, offset); + init = 1; + } + + 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') { + pen_up(); + not_done = 0; + break; + } + + last_x = x; + last_y = y; + cur = instbuf; + err = retrieve_inst(instbuf, &x, &y, &draw); + if (err == 0) { + last_state = state; + state = ERROR; + break; + } else { + last_state = state; + state = TURN; + break; + } + + case TURN: + delta_a = compute_turn_angle(last_x, last_y, x, y, angle); + angle += delta_a; + if (angle >= FULL_TURN) { + angle -= FULL_TURN; + } + left(delta_a); + if (draw) { + last_state = state; + state = DRAW; + break; + } else { + last_state = state; + state = MOVE; + break; + } + case DRAW: + dist = fabs(distance(last_x, last_y, x, y)); + dist = CAL_FACTOR*cal_time*(dist/(double)dim_x); + pen_down(); + forward(dist); + pen_up(); + last_state = state; + state = PARSE; + break; + case MOVE: + dist = fabs(distance(last_x, last_y, x, y)); + dist = CAL_FACTOR*cal_time*(dist/(double)dim_x); + forward(dist); + last_state = state; + state = PARSE; + break; + case ERROR: + robot_printf(0, "E:%d", last_state); + while(1); + } + } + /* while (1) { memset(instbuf+offset, 0, INST_BUF_SIZE-offset); bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file); @@ -140,7 +231,6 @@ } init = 1; - /* Compute turn angle and turn. */ delta_a = compute_turn_angle(last_x, last_y, x, y, angle); angle += delta_a; if (angle >= FULL_TURN) { @@ -152,20 +242,17 @@ dist = fabs(distance(last_x, last_y, x, y)); robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x)); - /* Put pen into position. */ if (draw) { pen_down(); } else { pen_up(); } - /* Compute drive time and move forward. */ forward(CAL_FACTOR*cal_time*(dist/(double)dim_x)); pen_up(); last_x = x; last_y = y; - /* Seek to find next instruction. */ next = strchr(cur, '\n'); if (next == NULL) { robot_printf(0, "nonext"); @@ -175,6 +262,7 @@ offset = instbuf+INST_BUF_SIZE-cur-1; memcpy(instbuf, cur, offset); } + */ robot_printf(0, "Done"); pi.stop(); pen_up();