Revision 39:cc8691700d2a, committed 2014-12-13

Comitter:

alecguertin

Date:

Sat Dec 13 07:35:57 2014 +0000

Parent:

38:69c7e86244e4

Child:

40:0199bad6c979

Commit message:

final

Changed in this revision

--- a/main.c	Sat Dec 13 03:34:04 2014 +0000
+++ b/main.c	Sat Dec 13 07:35:57 2014 +0000
@@ -26,194 +26,127 @@
 /** @brief Entry point. Main loop. */
 int main()
 {
-    FILE    *ps_file;
-    float   cal_time;
-    float   pos         = 0;
-    float   over_thresh = 0.05;
-    float   correction  = 0.2*DRIVE_SPEED;
-    int     instbuflen  = 250;
-    char    instbuf[instbuflen];
- 
-    // ---------------------------------------------------------
-    // First calibrate robot and figure out space dimensions.
-    pi.cls();
-    pi.locate(0,1);
-    pi.printf("%f mV", pi.battery());
-    pi.sensor_auto_calibrate();
-    timerWait(.2);
+    Timer       caltimer;
+    FILE        *ps_file;
+    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;
+    char        instbuf[INST_BUF_SIZE];
+    char        *cur, *next;
+    angle       =   0;
+    last_x      =   0;
+    last_y      =   0;
+    bytes_read  =   0;
+    offset      =   0;
+    int init = 0;
 
-    do {
-        pos = pi.line_position();
-        if(pos > over_thresh) { 
-            pi.right_motor(CAL_SPEED);
-            pi.left_motor(CAL_SPEED-correction);
-        } else if(pos < -over_thresh) {
-            pi.left_motor(CAL_SPEED);
-            pi.right_motor(CAL_SPEED-correction);
-        } else {
-            pi.right_motor(CAL_SPEED);
-            pi.left_motor(CAL_SPEED);
-        }
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("Pos: %f", pos);
-    } while(pos != -1 && pos > -0.3);
-    pi.stop();
-    if(pos != -1) {
-        timer.stop();
-    } else {
-        oled_1 = 1;
-        while(1);
+    /* 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");
+        while (1);
     }
-    left(180);
-    Timer caltimer;
+    left(HALF_TURN);
+
+    /*
+     * Find bottom left corner of frame and
+     * and measure length of frame side.
+     */
     caltimer.start();
-    do {
-        pos = pi.line_position();
-        if(pos > over_thresh) {
-            pi.right_motor(CAL_SPEED);
-            pi.left_motor(CAL_SPEED-correction);
-        } else if(pos < -over_thresh) {
-            pi.left_motor(CAL_SPEED);
-            pi.right_motor(CAL_SPEED-correction);
-        } else {
-            pi.right_motor(CAL_SPEED);
-            pi.left_motor(CAL_SPEED);
-        }
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("Pos: %f", pos);
-    } while(pos != -1 && pos <= 0.3);
+    corner = find_corner(0);
+    if (!corner) {
+        robot_printf(0, "no right\n");
+        while (1);
+    }
     caltimer.stop();
-    cal_time = caltimer.read_ms()*2;
-    pi.stop();
-    if(pos != -1) {
-        timer.stop();
-    } else {
-        oled_1 = 1;
-        while(1);
-    }
-    right(183);
-    timerWait(.2);
-    /*int wiggle_count = 0;
-    
-    for(    ; fabs(pos = pi.line_position()) > CLOSE_ENOUGH 
-            && wiggle_count < WIGGLE_MAX; wiggle_count++) {
-        pi.right((pos < 0 ? -.6*.4*CAL_SPEED : .6*.4*CAL_SPEED));
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("O: %f", pos);
-    }*/
-    pi.stop();
+    cal_time = caltimer.read_ms();
+
+    /* Back up into corner (coordinate x=0,y=0). */
+    right(HALF_TURN);
     timerWait(0.2);
     backward(400);
-    //while(1);
-    
-    // ---------------------------------------------------------
-    // DONE calibrating. Begin reading in coordinate
-    // file and run robot. 
-    size_t bytes_read = 0;
-    int err, x, y, last_x, last_y, delta_x, delta_y;
-    float delta_a;
-    int dim_x, dim_y;
-    int offset = 0;
-    char *cur, *next;
-    float angle;
-    double dist, theta;
-    angle = 0;
-    theta = 0;
-    last_x = 0;
-    last_y = 0;
+
+    /* Open instruction file. */
     ps_file = fopen("/local/test.ps", "r");
     if (ps_file == NULL) {
         return 1;
     }
 
-    /* PS parsing loop. */
-    memset(instbuf, 0, instbuflen);
-    bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
+    /* 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) {
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("sscanf1");
+        robot_printf(0, "sscanf1");
         while(1);
     }
     cur = strchr(instbuf, '\n');
     cur++;
-    offset = instbuf+instbuflen-cur-1;
+    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. 
+    /*
+     * File open and buffer setup. Begin reading instructions and
+     * moving robot. Refill buffer after each instruction. 
+     */
     while (1) {
-        memset(instbuf+offset, 0, instbuflen-offset);
-        bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
-        
+        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') {
-            pi.cls();
-            pi.locate(0,0);
-            pi.printf("%s", instbuf+1);
+            robot_printf(0, "%s", instbuf+1);
             oled_3 = 0;
             while(1);
         }
         
         cur = instbuf;
         err = retrieve_inst(instbuf, &x, &y, &draw);
-        
         if (err == 0) {
-            pi.cls();
-            pi.locate(0,0);
-            pi.printf("noinst");
+            robot_printf(0, "noinst");
             return 1;
         }
         
-        delta_x = x-last_x;
-        delta_y = y-last_y;
-        if (delta_y == 0) {
-            if (delta_x < 0) {
-                theta = 180;
-            } else {
-                theta = 0;
+        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();
             }
-        } else if (delta_x == 0) {
-            if (delta_y < 0) {
-                theta = -90;
-            } else {
-                theta = 90;
-            }
-        } else {
-            /* Compute turn angle and turn. */
-            theta = atan(((double) delta_y)/((double) delta_x));
-            theta *= 57.29;
-        }
-        if (delta_x < 0 && delta_y > 0) {
-            theta += 180;
-        }
-        delta_a = theta-angle;
-        
-        if (delta_x < 0 && delta_y < 0) {
-            delta_a += 180;
+            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;
         }
         
-        if (delta_a > 180) {
-            delta_a -= 360;
-        } else if (delta_a < -180) {
-            delta_a = 360 + delta_a;
-        }
+        init = 1;
+        /* Compute turn angle and turn. */
+        delta_a = compute_turn_angle(last_x, last_y, x, y, angle);
         angle += delta_a;
-        if (angle >= 360) {
-            angle -= 360;
+        if (angle >= FULL_TURN) {
+            angle -= FULL_TURN;
         }
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("a:%f", delta_a);
-        oled_3 = 0;
+        robot_printf(0, "a:%f", delta_a);
         left(delta_a);
 
         /* Put pen into position. */
@@ -224,44 +157,135 @@
         }
 
         /* Compute drive time and move forward. */
-        dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
-        if (dist < 0) {
-            dist *= -1;
-        }
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("d:%f", 0.55*cal_time*(dist/(double)dim_x));
-        forward(0.55*cal_time*(dist/(double)dim_x));
+        dist = fabs(distance(last_x, last_y, x, y));
+        robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x));
+        forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
         last_x = x;
         last_y = y;
+        oled_3 = 0;
+        
+        /* Seek to find next instruction. */
         next = strchr(cur, '\n');
         if (next == NULL) {
-            pi.cls();
-            pi.locate(0,0);
-            pi.printf("nonext");
+            robot_printf(0, "nonext");
             break;
         }
         cur = next+1;
-        offset = instbuf+instbuflen-cur-1;
+        offset = instbuf+INST_BUF_SIZE-cur-1;
         memcpy(instbuf, cur, offset);
     }
-    pi.cls();
-    pi.locate(0,0);
-    pi.printf("Done");
+    robot_printf(0, "Done");
     pi.stop(); 
     oled_3 = 0;
     while (1);
 }
 
+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\n", pos);
+    }
+    robot_printf(0, "f:%f\n", 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) {
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("nomatch");
+        robot_printf(0, "nomatch");
         return 0;
     }
     srch = strchr(buf, ' ');
@@ -273,9 +297,7 @@
     } else if (srch[0] == 'l') {
         *draw = 1;
     } else {
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("%.8s", srch);
+        robot_printf(0, "%.8s", srch);
         return 0;
     }
     return 1;
@@ -286,8 +308,7 @@
     Timer t;
     float ms = 0;
     float last_ms = 0;
-    pi.locate(0,0);
-    pi.printf("Fwd %d", amt);
+    robot_printf(0, "Fwd %d", amt);
     t.start();
     pi.left_motor(DRIVE_SPEED+.002);
     pi.right_motor(DRIVE_SPEED);
@@ -315,15 +336,28 @@
     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)
 {
-    deg += 2;
+    Timer t;
+    deg += DEGREE_CORRECTION;
     if(deg < 0) {
         return left(-1*deg);
     }
-    deg = deg;
-    Timer t;
-    float amt = ((((float)deg)/360)*TIME_FACT*1000);
+    float amt = ((((float)deg)/FULL_TURN)*TIME_FACT*1000);
     t.start();
     pi.right(TURN_SPEED);
     while(t.read_us() < amt);
@@ -335,17 +369,4 @@
     Timer t;
     t.start();
     while(t.read_us() < 1000000*seconds);
-}
-
-void left(float deg)
-{
-    if(deg < 0) {
-        return right(-1*deg);
-    }
-    float amt = (((float)deg)/360)*TIME_FACT*1000;
-    Timer t;
-    pi.left(TURN_SPEED);
-    t.start();
-    while(t.read_us() < amt);
-    pi.stop();
 }
\ No newline at end of file

--- a/main.h	Sat Dec 13 03:34:04 2014 +0000
+++ b/main.h	Sat Dec 13 07:35:57 2014 +0000
@@ -23,6 +23,19 @@
 #define CAL_SPEED .25       /**< Drive speed during calibration */
 #define CLOSE_ENOUGH .0008  /**< Threshold for calibration line centering */
 #define WIGGLE_MAX 30       /**< Max 'wiggles' during calibration */
+#define CORNER_THRESHOLD 0.3 /**< Threshold for checking if line position denotes a corner */
+#define INST_BUF_SIZE 250 /**< Size of input buffer for instructions */
+#define CORRECTION_SPEED 0.2*DRIVE_SPEED /**< Amount to change speed of one wheel when following line */
+#define CORRECTION_THRESHOLD 0.05 /**< Maximum tolerable deviation from line when following line */
+#define RAD_TO_DEG  57.29 /**< Factor to convert radians to degrees */
+#define FULL_TURN   360 /**< Degrees for a full turn */
+#define HALF_TURN   180 /**< Degrees for a half turn */
+#define QUARTER_TURN    90 /**< Degrees for a quarter (right angle) turn */
+#define CAL_FACTOR  1.1 /**< Scaling factor for time to drive */
+#define DEGREE_CORRECTION 0 /**< Amount (in degrees) added to the angle for each turn */
+#define SEC_TO_MSEC 1000 /**< Scaling factor to convert seconds to milliseconds */
+#define SEC_TO_USEC 1000000 /**< Scaling factor to convert seconds to microseconds */
+#define MSEC_TO_USEC 1000 /**< Scaling factor to convert milliseconds to microseconds */
 
 /** @brief Move the robot from its current position to (x,y) */
 void move(int x, int y, int draw);
@@ -80,4 +93,14 @@
  * @param[in]   amt     Time to wait, in seconds.
  */
 void timerWait(float amt);
+
+int find_corner(int left);
+
+void find_line();
+
+float distance(int x1, int y1, int x2, int y2);
+
+void robot_printf(int line, const char *format, ...);
+
+float compute_turn_angle(int last_x, int last_y, int x, int y, float angle);
 #endif
\ No newline at end of file