EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 44:fd755ef0f862
- Parent:
- 43:a0c9549a6f8a
- Child:
- 45:b0f1d06aa6df
diff -r a0c9549a6f8a -r fd755ef0f862 main.c --- a/main.c Thu Dec 18 04:22:24 2014 +0000 +++ b/main.c Thu Dec 18 05:20:51 2014 +0000 @@ -33,6 +33,7 @@ TURN, MOVE, DRAW, + FIND_LINE, FINISHED, }; @@ -43,9 +44,9 @@ FILE *ps_file; RobotState state, last_state; double dist; - float delta_a, angle, cal_time; + float delta_a, angle, cal_time, pos, last_pos; size_t bytes_read; - int corner, err, init, x, y, last_x, last_y, dim_x, dim_y, offset, right_found; + int corner, err, init, x, y, last_x, last_y, dim_x, dim_y, offset, right_found, complete; char instbuf[INST_BUF_SIZE]; char *cur, *next; angle = 0; @@ -57,56 +58,18 @@ offset = 0; init = 0; right_found = 0; - caltimer = Timer(); + delta_a = 0; + angle = 0; + cal_time = 0; + pos = 0; + last_pos = 0; + complete = 0; + cur = NULL; + next = NULL; + ps_file = NULL; + caltimer = Timer(); /* - robot_printf(0, "%f mV", pi.battery()); - - pi.sensor_auto_calibrate(); - timerWait(0.2); - - corner = find_corner(1); - if (!corner) { - robot_printf(0, "no left\n"); - while (1); - } - left(HALF_TURN); - - caltimer.start(); - corner = find_corner(0); - if (!corner) { - robot_printf(0, "no right\n"); - while (1); - } - caltimer.stop(); - cal_time = caltimer.read_ms(); - - right(HALF_TURN); - timerWait(0.2); - backward(400); - - ps_file = fopen("/local/test.ps", "r"); - if (ps_file == NULL) { - return 1; - } - - memset(instbuf, 0, INST_BUF_SIZE); - bytes_read = fread(instbuf, sizeof(char), INST_BUF_SIZE-1, ps_file); - if (bytes_read == 0) { - return 1; - } - - err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y); - if (err != 2) { - robot_printf(0, "sscanf1"); - while(1); - } - cur = strchr(instbuf, '\n'); - cur++; - 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. */ @@ -114,15 +77,21 @@ last_state = LINE_CAL; while (1) { switch (state) { + + // Calibrate the m3pi line sensors case LINE_CAL: pi.sensor_auto_calibrate(); timerWait(0.2); last_state = state; state = FIND_CORNER; break; + + // Find a corner of the frame case FIND_CORNER: last_state = state; - if (right_found) { + // Corner we are looking for specified by right_found + // If we already found the right corner, measure frame + if (right_found && !complete) { caltimer.start(); } corner = find_corner(1-right_found); @@ -130,15 +99,20 @@ state = ERROR; break; } - if (right_found) { + if (right_found && !complete) { caltimer.stop(); cal_time = caltimer.read_ms(); } state = REV_TURN; break; + + // Reverse robot to face other corner of frame case REV_TURN: last_state = state; - if (right_found) { + if (complete) { + right(HALF_TURN); + state = FINISHED; + } else if (right_found) { right(HALF_TURN); state = DRAW_INIT; } else { @@ -147,24 +121,30 @@ right_found = 1; } break; + + // Setup for drawing stage case DRAW_INIT: + // Backup to start at (0,0) timerWait(0.2); backward(400); last_state = state; + // Open the instructions file ps_file = fopen("/local/test.ps", "r"); if (ps_file == NULL) { state = ERROR; break; } + // Read start 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) { state = ERROR; break; } - + + // Find dimensions of PS instructions err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y); if (err != 2) { state = ERROR; @@ -176,9 +156,15 @@ memcpy(instbuf, cur, offset); state = PARSE; break; + + // Retrieve the next instruction + // Update robot vars to give desired position case PARSE: + // Parsing uses in place buffer + // next instruction is at start of buffer last_state = state; if (init) { + // Advance to next instruction next = strchr(cur, '\n'); if (next == NULL) { state = ERROR; @@ -194,12 +180,15 @@ bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file); if (instbuf[0] == '\0') { pen_up(); - state = FINISHED; + state = FIND_LINE; break; } + // Update robot position last_x = x; last_y = y; + + // Get next instruction cur = instbuf; err = retrieve_inst(instbuf, &x, &y, &draw); if (err == 0) { @@ -210,13 +199,23 @@ break; } + // Move angle needed to reach desired position case TURN: last_state = state; + + // Computed necessary angle delta_a = compute_turn_angle(last_x, last_y, x, y, angle); + + // Update robot angle angle += delta_a; if (angle >= FULL_TURN) { angle -= FULL_TURN; } + if (angle <= -FULL_TURN) { + angle += FULL_TURN; + } + + // Move desired angle left(delta_a); if (draw) { state = DRAW; @@ -225,108 +224,72 @@ state = MOVE; break; } + + // Move to desired position while drawing case DRAW: + // Compute Euclidean distance between points dist = fabs(distance(last_x, last_y, x, y)); pen_down(); + // Move desired distance forward(CAL_FACTOR*cal_time*(dist/(double)dim_x)); pen_up(); last_state = state; state = PARSE; break; + + // Move to desired position w/o drawing + // (lifting pen and starting somewhere else) case MOVE: + // Compute Euclidean distance dist = fabs(distance(last_x, last_y, x, y)); + // Move desired distance forward(CAL_FACTOR*cal_time*(dist/(double)dim_x)); last_state = state; state = PARSE; break; + + case FIND_LINE: + if (angle > 0) { + right(angle+QUARTER_TURN); + } else { + right(angle+QUARTER_TURN); + } + pos = pi.line_position(); + last_pos = pos; + pi.forward(DRIVE_SPEED/2); + while (pos < 0.6 && fabs(pos-last_pos) > 0.1) { + pos = pi.line_position(); + last_pos = pos; + robot_printf(0, "%f", pos); + } + pi.stop(); + forward(300); + right(QUARTER_TURN); + last_state = state; + state = FIND_CORNER; + break; + + // Done drawing - alert user case FINISHED: + pi.stop(); + pen_up(); robot_printf(0, "done"); while(1); + + // Error - alert user case ERROR: + pi.stop(); + pen_up(); robot_printf(0, "E:%d", last_state); while(1); + + // Unrecognized state - go to ERROR default: last_state = state; state = ERROR; break; } } - /* - while (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') { - robot_printf(0, "%s", instbuf+1); - pen_up(); - while(1); - } - - cur = instbuf; - err = retrieve_inst(instbuf, &x, &y, &draw); - if (err == 0) { - robot_printf(0, "noinst"); - return 1; - } - - 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(); - } - 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; - } - - init = 1; - delta_a = compute_turn_angle(last_x, last_y, x, y, angle); - angle += delta_a; - if (angle >= FULL_TURN) { - angle -= FULL_TURN; - } - robot_printf(0, "a:%f", delta_a); - left(delta_a); - - dist = fabs(distance(last_x, last_y, x, y)); - robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x)); - - if (draw) { - pen_down(); - } else { - pen_up(); - } - - forward(CAL_FACTOR*cal_time*(dist/(double)dim_x)); - pen_up(); - last_x = x; - last_y = y; - - next = strchr(cur, '\n'); - if (next == NULL) { - robot_printf(0, "nonext"); - break; - } - cur = next+1; - offset = instbuf+INST_BUF_SIZE-cur-1; - memcpy(instbuf, cur, offset); - } - */ - robot_printf(0, "Done"); - pi.stop(); - pen_up(); - while (1); } float compute_turn_angle(int last_x, int last_y, int x, int y, float angle)