Revision 43:a0c9549a6f8a, committed 2014-12-18

Comitter:

alecguertin

Date:

Thu Dec 18 04:22:24 2014 +0000

Parent:

42:727612987d77

Child:

44:fd755ef0f862

Commit message:

added states for initialization

Changed in this revision

--- a/main.c	Thu Dec 18 03:53:42 2014 +0000
+++ b/main.c	Thu Dec 18 04:22:24 2014 +0000
@@ -25,6 +25,10 @@
 
 enum RobotState {
     ERROR,
+    LINE_CAL,
+    FIND_CORNER,
+    REV_TURN,
+    DRAW_INIT,
     PARSE,
     TURN,
     MOVE,
@@ -41,7 +45,7 @@
     double      dist;
     float       delta_a, angle, cal_time;
     size_t      bytes_read;
-    int         corner, err, init, x, y, last_x, last_y, dim_x, dim_y, offset;
+    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;
@@ -52,16 +56,15 @@
     bytes_read  =   0;
     offset      =   0;
     init        =   0;
+    right_found =   0;
     caltimer = Timer();
-
-    /* First calibrate robot and figure out space dimensions. */
+    
+    /*
     robot_printf(0, "%f mV", pi.battery());
 
-    /* Calibrate robot reflective sensors to line. */
     pi.sensor_auto_calibrate();
     timerWait(0.2);
 
-    /* Find left bottom right corner of frame. */
     corner = find_corner(1);
     if (!corner) {
         robot_printf(0, "no left\n");
@@ -69,10 +72,6 @@
     }
     left(HALF_TURN);
 
-    /*
-     * Find bottom left corner of frame and
-     * and measure length of frame side.
-     */
     caltimer.start();
     corner = find_corner(0);
     if (!corner) {
@@ -82,25 +81,21 @@
     caltimer.stop();
     cal_time = caltimer.read_ms();
 
-    /* Back up into corner (coordinate x=0,y=0). */
     right(HALF_TURN);
     timerWait(0.2);
     backward(400);
 
-    /* Open instruction file. */
     ps_file = fopen("/local/test.ps", "r");
     if (ps_file == NULL) {
         return 1;
     }
 
-    /* Read beginning 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) {
         return 1;
     }
     
-    /* Read first line of file - the dimensions of the frame. */
     err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
     if (err != 2) {
         robot_printf(0, "sscanf1");
@@ -110,19 +105,82 @@
     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. 
      */
-    state = PARSE;
-    last_state = PARSE;
+    state = LINE_CAL;
+    last_state = LINE_CAL;
     while (1) {
         switch (state) {
+            case LINE_CAL:
+                pi.sensor_auto_calibrate();
+                timerWait(0.2);
+                last_state = state;
+                state = FIND_CORNER;
+                break;
+            case FIND_CORNER:
+                last_state = state;
+                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;
+            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;
+            case DRAW_INIT:
+                timerWait(0.2);
+                backward(400);
+                last_state = state;
+            
+                ps_file = fopen("/local/test.ps", "r");
+                if (ps_file == NULL) {
+                    state = ERROR;
+                    break;
+                }
+            
+                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;
+                }
+                
+                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;
             case PARSE:
+                last_state = state;
                 if (init) {
                     next = strchr(cur, '\n');
                     if (next == NULL) {
-                        last_state = state;
                         state = ERROR;
                         break;
                     }
@@ -136,7 +194,6 @@
                 bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file);
                 if (instbuf[0] == '\0') {
                     pen_up();
-                    last_state = state;
                     state = FINISHED;
                     break;
                 }
@@ -146,16 +203,15 @@
                 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:
+                last_state = state;
                 delta_a = compute_turn_angle(last_x, last_y, x, y, angle);
                 angle += delta_a;
                 if (angle >= FULL_TURN) {
@@ -163,11 +219,9 @@
                 }
                 left(delta_a);
                 if (draw) {
-                    last_state = state;
                     state = DRAW;
                     break;
                 } else {
-                    last_state = state;
                     state = MOVE;
                     break;
                 }
@@ -186,10 +240,10 @@
                 state = PARSE;
                 break;
             case FINISHED:
-                robot_printf(0, "done\n");
+                robot_printf(0, "done");
                 while(1);
             case ERROR:
-                robot_printf(0, "E:%d\n", last_state);
+                robot_printf(0, "E:%d", last_state);
                 while(1);
             default:
                 last_state = state;
@@ -339,9 +393,9 @@
     pos = pos1;
     while (fabs(pos1-pos) < 0.1 || pos == -1) {
         pos = pi.line_position();
-        robot_printf(0, "%f\n", pos);
+        robot_printf(0, "%f", pos);
     }
-    robot_printf(0, "f:%f\n", pos);
+    robot_printf(0, "f:%f", pos);
     pi.stop();
 }