main.c

00001 /**
00002  * @file    driver.c
00003  * @brief   Primary control logic for robot.
00004  *
00005  * @author  John Wilkey
00006  * @author  Alec Guertin
00007  * @author  Chester Chu
00008  */
00009 #include "main.h"
00010 
00011 m3pi        pi;                 /**< m3pi Object for all control operations. */
00012 Timer       timer;              /**< Timer used to wait in timerWait. */
00013 DigitalIn   start_button(p21);  /**< Pi start button input on mBed pin 32 */
00014 int         draw;               /**< Boolean for drawing/moving. */
00015 
00016 /* 4 mBed onboard LEDs */
00017 DigitalOut  oled_1(LED1), oled_2(LED2), oled_3(LED3), oled_4(LED4);
00018 
00019 /* 8 LEDs driven from mBed digital outs. */
00020 DigitalOut  pin13(p13), pin14(p14), pin15(p15), pin16(p16),
00021             pin17(p17), pin18(p18), pin19(p19), pin20(p20);         
00022 
00023 /* Local File System */
00024 LocalFileSystem local("local");
00025 
00026 /* States specifying the robot's behavior. */
00027 enum RobotState {
00028     ERROR,
00029     LINE_CAL,
00030     FIND_CORNER,
00031     REV_TURN,
00032     DRAW_INIT,
00033     PARSE,
00034     TURN,
00035     MOVE,
00036     DRAW,
00037     FINISHED,
00038 };
00039 
00040 /** @brief Entry point. Main loop. */
00041 int main()
00042 {
00043     Timer       caltimer;
00044     FILE        *ps_file;
00045     RobotState  state, last_state;
00046     double      dist;
00047     float       delta_a, angle, cal_time, pos, last_pos;
00048     size_t      bytes_read;
00049     int         corner, err, init, x, y, last_x, last_y, dim_x, dim_y, offset, right_found;
00050     char        instbuf[INST_BUF_SIZE];
00051     char        *cur, *next;
00052     angle       =   0;
00053     x           =   0;
00054     y           =   0;
00055     last_x      =   0;
00056     last_y      =   0;
00057     bytes_read  =   0;
00058     offset      =   0;
00059     init        =   0;
00060     right_found =   0;
00061     delta_a     =   0;
00062     angle       =   0;
00063     cal_time    =   0;
00064     pos         =   0;
00065     last_pos    =   0;
00066     cur         =   NULL;
00067     next        =   NULL;
00068     ps_file     =   NULL;
00069     caltimer    =   Timer();
00070     
00071     /*
00072      * File open and buffer setup. Begin reading instructions and
00073      * moving robot. Refill buffer after each instruction. 
00074      */
00075     state = LINE_CAL;
00076     last_state = LINE_CAL;
00077     while (1) {
00078         switch (state) {
00079 
00080             // Calibrate the m3pi line sensors
00081             case LINE_CAL:
00082                 pi.sensor_auto_calibrate();
00083                 timerWait(0.2);
00084                 last_state = state;
00085                 state = FIND_CORNER;
00086                 break;
00087 
00088             // Find a corner of the frame
00089             case FIND_CORNER:
00090                 last_state = state;
00091                 // Corner we are looking for specified by right_found
00092                 // If we already found the right corner, measure frame
00093                 if (right_found) {
00094                     caltimer.start();
00095                 }
00096                 corner = find_corner(1-right_found);
00097                 if (!corner) {
00098                     state = ERROR;
00099                     break;
00100                 }
00101                 if (right_found) {
00102                     caltimer.stop();
00103                     cal_time = caltimer.read_ms();
00104                 }
00105                 state = REV_TURN;
00106                 break;
00107 
00108             // Reverse robot to face other corner of frame
00109             case REV_TURN:
00110                 last_state = state;
00111                 if (right_found) {
00112                     right(HALF_TURN);
00113                     state = DRAW_INIT;
00114                 } else {
00115                     left(HALF_TURN);
00116                     state = FIND_CORNER;
00117                     right_found = 1;
00118                 }
00119                 break;
00120 
00121             // Setup for drawing stage
00122             case DRAW_INIT:
00123                 // Backup to start at (0,0)
00124                 timerWait(0.2);
00125                 backward(400);
00126                 last_state = state;
00127             
00128                 // Open the instructions file
00129                 ps_file = fopen("/local/control.ps", "r");
00130                 if (ps_file == NULL) {
00131                     state = ERROR;
00132                     break;
00133                 }
00134             
00135                 // Read start of file into buffer
00136                 memset(instbuf, 0, INST_BUF_SIZE);
00137                 bytes_read = fread(instbuf, sizeof(char), INST_BUF_SIZE-1, ps_file);
00138                 if (bytes_read == 0) {
00139                     state = ERROR;
00140                     break;
00141                 }
00142 
00143                 // Find dimensions of PS instructions
00144                 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
00145                 if (err != 2) {
00146                     state = ERROR;
00147                     break;
00148                 }
00149                 cur = strchr(instbuf, '\n');
00150                 cur++;
00151                 offset = instbuf+INST_BUF_SIZE-cur-1;
00152                 memcpy(instbuf, cur, offset);
00153                 state = PARSE;
00154                 break;
00155 
00156             // Retrieve the next instruction
00157             // Update robot vars to give desired position
00158             case PARSE:
00159                 // Parsing uses in place buffer
00160                 // next instruction is at start of buffer
00161                 last_state = state;
00162                 if (init) {
00163                     // Advance to next instruction
00164                     next = strchr(cur, '\n');
00165                     if (next == NULL) {
00166                         state = ERROR;
00167                         break;
00168                     }
00169                     cur = next+1;
00170                     offset = instbuf+INST_BUF_SIZE-cur-1;
00171                     memcpy(instbuf, cur, offset);
00172                 }
00173                 init = 1;
00174     
00175                 memset(instbuf+offset, 0, INST_BUF_SIZE-offset);
00176                 bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file);
00177                 if (instbuf[0] == '\0') {
00178                     pen_up();
00179                     state = FINISHED;
00180                     break;
00181                 }
00182 
00183                 // Update robot position
00184                 last_x = x;
00185                 last_y = y;
00186 
00187                 // Get next instruction
00188                 cur = instbuf;
00189                 err = retrieve_inst(instbuf, &x, &y, &draw);
00190                 if (err == 0) {
00191                     state = ERROR;
00192                     break;
00193                 } else {
00194                     state = TURN;
00195                     break;
00196                 }
00197                 
00198             // Move angle needed to reach desired position
00199             case TURN:
00200                 last_state = state;
00201 
00202                 // Computed necessary angle                
00203                 delta_a = compute_turn_angle(last_x, last_y, x, y, angle);
00204 
00205                 // Update robot angle
00206                 angle += delta_a;
00207                 if (angle >= FULL_TURN) {
00208                     angle -= FULL_TURN;
00209                 }
00210                 if (angle <= -FULL_TURN) {
00211                     angle += FULL_TURN;
00212                 }
00213 
00214                 // Move desired angle
00215                 left(delta_a);
00216                 if (draw) {
00217                     state = DRAW;
00218                     break;
00219                 } else {
00220                     state = MOVE;
00221                     break;
00222                 }
00223 
00224             // Move to desired position while drawing
00225             case DRAW:
00226                 // Compute Euclidean distance between points
00227                 dist = fabs(distance(last_x, last_y, x, y));
00228                 pen_down();
00229                 // Move desired distance
00230                 forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
00231                 pen_up();
00232                 last_state = state;
00233                 state = PARSE;
00234                 break;
00235 
00236             // Move to desired position w/o drawing
00237             // (lifting pen and starting somewhere else)
00238             case MOVE:
00239                 // Compute Euclidean distance
00240                 dist = fabs(distance(last_x, last_y, x, y));
00241                 // Move desired distance
00242                 forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
00243                 last_state = state;
00244                 state = PARSE;
00245                 break;
00246 
00247             // Done drawing - alert user
00248             case FINISHED:
00249                 pi.stop(); 
00250                 pen_up();
00251                 robot_printf(0, "done");
00252                 while(1);
00253 
00254             // Error - alert user
00255             case ERROR:
00256                 pi.stop(); 
00257                 pen_up();
00258                 robot_printf(0, "E:%d", last_state);
00259                 while(1);
00260 
00261             // Unrecognized state - go to ERROR
00262             default:
00263                 last_state = state;
00264                 state = ERROR;
00265                 break;
00266         }
00267     }
00268 }
00269 
00270 float compute_turn_angle(int last_x, int last_y, int x, int y, float angle)
00271 {
00272     int delta_x, delta_y;
00273     float theta, delta_a;
00274     delta_x = x-last_x;
00275     delta_y = y-last_y;
00276     if (delta_y == 0) {
00277         if (delta_x < 0) {
00278             theta = HALF_TURN;
00279         } else {
00280             theta = 0;
00281         }
00282     } else if (delta_x == 0) {
00283         if (delta_y < 0) {
00284             theta = -QUARTER_TURN;
00285         } else {
00286             theta = QUARTER_TURN;
00287         }
00288     } else {
00289         theta = atan(((double) delta_y)/((double) delta_x));
00290         theta *= RAD_TO_DEG;
00291     }
00292     if (delta_x < 0 && delta_y > 0) {
00293         theta += HALF_TURN;
00294     }
00295     delta_a = theta-angle;
00296 
00297     if (delta_x < 0 && delta_y < 0) {
00298         delta_a += HALF_TURN;
00299     }
00300     
00301     if (delta_a > HALF_TURN) {
00302         delta_a -= FULL_TURN;
00303     } else if (delta_a < -HALF_TURN) {
00304         delta_a = FULL_TURN + delta_a;
00305     }
00306     return delta_a;
00307 }
00308 
00309 void robot_printf(int line, const char *format, ...)
00310 {
00311     char buf[18];
00312     va_list args;
00313     pi.cls();
00314     pi.locate(0,line);
00315     va_start(args, format);
00316     vsprintf(buf, format, args);
00317     pi.printf("%s", buf);
00318     va_end(args);
00319 }
00320 
00321 float distance(int x1, int y1, int x2, int y2)
00322 {
00323     return sqrt(pow((double) (x2-x1),2) + pow((double) (y2-y1), 2));
00324 }
00325 
00326 void find_line()
00327 {
00328     float pos, pos1;
00329     pi.forward(DRIVE_SPEED);
00330     pos1 = pi.line_position();
00331     pos = pos1;
00332     while (fabs(pos1-pos) < 0.1 || pos == -1) {
00333         pos = pi.line_position();
00334         robot_printf(0, "%f", pos);
00335     }
00336     robot_printf(0, "f:%f", pos);
00337     pi.stop();
00338 }    
00339 
00340 int find_corner(int left)
00341 {
00342     float pos;
00343     float threshold = CORNER_THRESHOLD;
00344     if (left) {
00345         threshold *= -1;
00346     }
00347     do {
00348         pos = pi.line_position();
00349         if(pos > CORRECTION_THRESHOLD) { 
00350             pi.right_motor(CAL_SPEED);
00351             pi.left_motor(CAL_SPEED-CORRECTION_SPEED);
00352         } else if(pos < -CORRECTION_THRESHOLD) {
00353             pi.left_motor(CAL_SPEED);
00354             pi.right_motor(CAL_SPEED-CORRECTION_SPEED);
00355         } else {
00356             pi.right_motor(CAL_SPEED);
00357             pi.left_motor(CAL_SPEED);
00358         }
00359         robot_printf(0, "Pos: %f", pos);
00360     } while(pos != -1 && ((left && pos > threshold) || (!left && pos < threshold)));
00361     pi.stop();
00362     if(pos == -1) {
00363         oled_1 = 1;
00364         return 0;
00365     }
00366     return 1;
00367 }
00368 
00369 int retrieve_inst(char *buf, int *x, int *y, int *draw)
00370 {
00371     int matches;
00372     char *srch;
00373     matches = sscanf(buf, "%d %d", x, y);
00374     if (matches != 2) {
00375         robot_printf(0, "nomatch");
00376         return 0;
00377     }
00378     srch = strchr(buf, ' ');
00379     srch++;
00380     srch = strchr(srch, ' ');
00381     srch++;
00382     if (srch[0] == 'm') {
00383         *draw = 0;
00384     } else if (srch[0] == 'l') {
00385         *draw = 1;
00386     } else {
00387         robot_printf(0, "%.8s", srch);
00388         return 0;
00389     }
00390     return 1;
00391 }
00392 
00393 void forward(int amt)
00394 {
00395     Timer t;
00396     float ms = 0;
00397     float last_ms = 0;
00398     robot_printf(0, "Fwd %d", amt);
00399     t.start();
00400     pi.left_motor(DRIVE_SPEED+.002);
00401     pi.right_motor(DRIVE_SPEED);
00402     ms = t.read_ms();
00403     while(ms < amt) {
00404         if (ms-last_ms > 500) {
00405             t.stop();
00406             right(1.5);
00407             last_ms = ms;
00408             t.start();
00409             pi.left_motor(DRIVE_SPEED+.002);
00410             pi.right_motor(DRIVE_SPEED);
00411         }
00412         ms = t.read_ms();
00413     }
00414     pi.stop();
00415 }
00416 
00417 void backward(int amt)
00418 {
00419     Timer t;
00420     t.start();
00421     pi.backward(.5*DRIVE_SPEED);
00422     while(t.read_ms() < amt);
00423     pi.stop();
00424 }
00425 
00426 void left(float deg)
00427 {
00428     Timer t;
00429     deg += DEGREE_CORRECTION;
00430     if(deg < 0) {
00431         return right(-1*deg);
00432     }
00433     float amt = (((float)deg)/FULL_TURN)*TIME_FACT*1000;
00434     pi.left(TURN_SPEED);
00435     t.start();
00436     while(t.read_us() < amt);
00437     pi.stop();
00438 }
00439 
00440 void right(float deg)
00441 {
00442     Timer t;
00443     deg += DEGREE_CORRECTION;
00444     if(deg < 0) {
00445         return left(-1*deg);
00446     }
00447     float amt = ((((float)deg)/FULL_TURN)*TIME_FACT*1000);
00448     t.start();
00449     pi.right(TURN_SPEED);
00450     while(t.read_us() < amt);
00451     pi.stop();
00452 }
00453 
00454 void timerWait(float seconds)
00455 {
00456     Timer t;
00457     t.start();
00458     while(t.read_us() < 1000000*seconds);
00459 }
00460 
00461 void pen_down()
00462 {
00463     oled_3 = 1;
00464 }
00465 
00466 void pen_up()
00467 {
00468     oled_3 = 0;
00469 }