Revision 41:1b6d8664d7a7, committed 2014-12-18

Comitter:

alecguertin

Date:

Thu Dec 18 03:31:58 2014 +0000

Parent:

40:0199bad6c979

Child:

42:727612987d77

Commit message:

added fsm logic to drawing

Changed in this revision

--- 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();