EE149
/
FinalProject
Final Project files for mBed development.
main.c
- Committer:
- alecguertin
- Date:
- 2014-12-19
- Revision:
- 45:b0f1d06aa6df
- Parent:
- 44:fd755ef0f862
- Child:
- 46:a3b9c0fe8f36
File content as of revision 45:b0f1d06aa6df:
/** * @file driver.c * @brief Primary control logic for robot. * * @author John Wilkey * @author Alec Guertin * @author Chester Chu */ #include "main.h" m3pi pi; /**< m3pi Object for all control operations. */ Timer timer; /**< Timer used to wait in timerWait. */ DigitalIn start_button(p21); /**< Pi start button input on mBed pin 32 */ int draw; /**< Boolean for drawing/moving. */ /* 4 mBed onboard LEDs */ DigitalOut oled_1(LED1), oled_2(LED2), oled_3(LED3), oled_4(LED4); /* 8 LEDs driven from mBed digital outs. */ DigitalOut pin13(p13), pin14(p14), pin15(p15), pin16(p16), pin17(p17), pin18(p18), pin19(p19), pin20(p20); /* Local File System */ LocalFileSystem local("local"); /* States specifying the robot's behavior. */ enum RobotState { ERROR, LINE_CAL, FIND_CORNER, REV_TURN, DRAW_INIT, PARSE, TURN, MOVE, DRAW, FINISHED, }; /** @brief Entry point. Main loop. */ int main() { Timer caltimer; FILE *ps_file; RobotState state, last_state; double dist; 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; 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; init = 0; right_found = 0; delta_a = 0; angle = 0; cal_time = 0; pos = 0; last_pos = 0; cur = NULL; next = NULL; ps_file = NULL; caltimer = Timer(); /* * File open and buffer setup. Begin reading instructions and * moving robot. Refill buffer after each instruction. */ state = LINE_CAL; 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; // Corner we are looking for specified by right_found // If we already found the right corner, measure frame if (right_found) { caltimer.start(); } corner = find_corner(1-right_found); if (!corner) { state = ERROR; break; } if (right_found) { 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) { right(HALF_TURN); state = DRAW_INIT; } else { left(HALF_TURN); state = FIND_CORNER; 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; break; } cur = strchr(instbuf, '\n'); cur++; offset = instbuf+INST_BUF_SIZE-cur-1; 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; 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(); state = FINISHED; 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) { state = ERROR; break; } else { state = TURN; 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; break; } else { 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; // 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; } } } 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", pos); } robot_printf(0, "f:%f", 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) { robot_printf(0, "nomatch"); return 0; } srch = strchr(buf, ' '); srch++; srch = strchr(srch, ' '); srch++; if (srch[0] == 'm') { *draw = 0; } else if (srch[0] == 'l') { *draw = 1; } else { robot_printf(0, "%.8s", srch); return 0; } return 1; } void forward(int amt) { Timer t; float ms = 0; float last_ms = 0; robot_printf(0, "Fwd %d", amt); t.start(); pi.left_motor(DRIVE_SPEED+.002); pi.right_motor(DRIVE_SPEED); ms = t.read_ms(); while(ms < amt) { if (ms-last_ms > 500) { t.stop(); right(1.5); last_ms = ms; t.start(); pi.left_motor(DRIVE_SPEED+.002); pi.right_motor(DRIVE_SPEED); } ms = t.read_ms(); } pi.stop(); } void backward(int amt) { Timer t; t.start(); pi.backward(.5*DRIVE_SPEED); while(t.read_ms() < amt); 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) { Timer t; deg += DEGREE_CORRECTION; if(deg < 0) { return left(-1*deg); } float amt = ((((float)deg)/FULL_TURN)*TIME_FACT*1000); t.start(); pi.right(TURN_SPEED); while(t.read_us() < amt); pi.stop(); } void timerWait(float seconds) { Timer t; t.start(); while(t.read_us() < 1000000*seconds); } void pen_down() { oled_3 = 1; } void pen_up() { oled_3 = 0; }