Revision 44:fd755ef0f862, committed 2014-12-18

Comitter:

alecguertin

Date:

Thu Dec 18 05:20:51 2014 +0000

Parent:

43:a0c9549a6f8a

Child:

45:b0f1d06aa6df

Commit message:

trying find_line

Changed in this revision

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