Revision 22:46b9d9b2e35c, committed 2014-12-07

Comitter:

alecguertin

Date:

Sun Dec 07 04:39:33 2014 +0000

Parent:

21:0c80a5d89ea3

Child:

23:e4616259a7f0

Commit message:

reads instructions correctly

Changed in this revision

--- a/main.c	Sat Dec 06 22:04:53 2014 +0000
+++ b/main.c	Sun Dec 07 04:39:33 2014 +0000
@@ -158,7 +158,8 @@
         return 1;
     }
     right(180);
-    wait(1);
+    Timer caltimer;
+    caltimer.start();
     do {
         pos = pi.line_position();
         if(pos > over_thresh) {
@@ -175,6 +176,8 @@
         pi.locate(0,0);
         pi.printf("Pos: %f", pos);
     } while(pos != -1 && pos <= 0.3);
+    caltimer.stop();
+    cal_time = caltimer.read_ms();
     pi.stop();
     if(pos != -1) {
         oled_1 = 1;
@@ -187,7 +190,52 @@
         return 1;
     }
     right(180);
-    timerWait(.07);
+    timerWait(.5);
+    while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
+        pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
+        // timerWait(.08);
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("O: %f", pos);
+        // pi.stop();
+        // timerWait(.2);
+    }
+    pi.stop();
+    timerWait(2);
+    /*
+    pos = pi.line_position();
+    do {
+        pi.backward(CAL_SPEED);
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("p:%f", pos);
+        pos = pi.line_position();
+    } while (pos != -1 && pos >= -0.25);
+    /**/
+    backward(500);
+    timerWait(.5);
+    while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
+        pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
+        // timerWait(.08);
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("O: %f", pos);
+        // pi.stop();
+        // timerWait(.2);
+    }
+    pi.stop();
+    timerWait(2);
+    /*
+    pos = pi.line_position();
+    do {
+        pi.forward(CAL_SPEED);
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("p:%f", pos);
+        pos = pi.line_position();
+    } while (pos <= -0.07);
+    timerWait(0.25);
+    pi.stop();
     pos = pi.line_position();
     while(fabs(pos) > CLOSE_ENOUGH) {
         pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
@@ -199,8 +247,8 @@
         pos = pi.line_position();
         timerWait(.2);
     }
-    timerWait(2);
-    
+    /**/
+    timerWait(1);
     //
     // Pivot 180 degrees to go to the starting position.
     /*
@@ -269,7 +317,8 @@
     // Main program loop.
     // robot_loop();
     size_t bytes_read = 0;
-    int err, x, y, last_x, last_y, delta_x, delta_y, delta_a;
+    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;
@@ -289,6 +338,9 @@
     /* PS parsing loop. */
     memset(instbuf, 0, instbuflen);
     bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
+    if (bytes_read == 0) {
+        return 1;
+    }
     pi.cls();
     pi.locate(0,0);
     pi.printf("%.7s", instbuf);
@@ -301,7 +353,7 @@
     }
     cur = strchr(instbuf, '\n');
     cur++;
-    offset = instbuf+instbuflen-cur;
+    offset = instbuf+instbuflen-cur-1;
     memcpy(instbuf, cur, offset);
     while (1) {
         memset(instbuf+offset, 0, instbuflen-offset);
@@ -310,75 +362,68 @@
             pi.cls();
             pi.locate(0,0);
             pi.printf("bytes0");
-            break;
+            timerWait(2);
         }
         cur = instbuf;
-        while (cur[0] != '\0') {
-            err = retrieve_inst(instbuf, &x, &y, &draw);
-            if (err == 0) {
-                pi.cls();
-                pi.locate(0,0);
-                pi.printf("noinst");
-                return 1;
-            }
-            delta_x = x-last_x;
-            delta_y = y-last_y;
-
-            /* Compute turn angle and turn. */
-            theta = tan((double) delta_x/delta_y);
-            theta *= 57.2957795;
-            delta_a = theta-angle;
+        err = retrieve_inst(instbuf, &x, &y, &draw);
+        if (err == 0) {
             pi.cls();
             pi.locate(0,0);
-            pi.printf("%d", delta_a);
-            if (delta_a > 0) {
-                left(delta_a);
-            } else {
-                right(delta_a);
-            }
-          
-            /* Put pen into position. */
-            if (draw) {
-                oled_1 = 1;
-            } else {
-                oled_1 = 0;
-            }
-            
-            /* Compute drive time and move forward. */
-            dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
-            if (dist < 0) {
-                dist *= -1;
-            }
-            timerWait(.2);
-            forward(1500);
-            right(90);
-            timerWait(.2);
-            forward(1500);
-            right(90);
-            timerWait(.2);
-            forward(1500);
-            right(90);
-            timerWait(.2);
-            forward(1500);
-            right(90);
-            timerWait(.2);
+            pi.printf("noinst");
+            return 1;
+        }
+        delta_x = x-last_x;
+        delta_y = y-last_y;
+
+        /* Compute turn angle and turn. */
+        theta = atan(((double) delta_x)/((double) delta_y));
+        theta *= 57.29;
+        delta_a = theta-angle;
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("a:%f", delta_a);
+        timerWait(2);
+        if (delta_a > 0) {
+            left(delta_a);
+        }
 
-            last_x = x;
-            last_y = y;
-            next = strchr(cur, '\n');
-            if (next == NULL) {
-                pi.cls();
-                pi.locate(0,0);
-                pi.printf("nonext");
-                break;
-            }
-            cur = next+1;
+        /* Put pen into position. */
+        if (draw) {
+            oled_1 = 1;
+        } else {
+            oled_1 = 0;
+        }
+
+        /* 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.4*cal_time*(dist/(double)dim_x));
+        timerWait(2);
+        forward(0.4*cal_time*(dist/(double)dim_x));
+
+        last_x = x;
+        last_y = y;
+        next = strchr(cur, '\n');
+        if (next == NULL) {
             pi.cls();
             pi.locate(0,0);
-            pi.printf("waiting!");
+            pi.printf("nonext");
+            break;
         }
-        offset = instbuf+instbuflen-cur;
+        cur = next+1;
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("waiting!");
+        offset = instbuf+instbuflen-cur-1;
         memcpy(instbuf, cur, offset);
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("%.08s", instbuf);
+        timerWait(2);
     }
     //
     // We should never reach this point!
@@ -430,7 +475,6 @@
     while(t.read_ms() < (amt-500));
     pi.left_motor(.7*DRIVE_SPEED+.0023);
     pi.right_motor(.7*DRIVE_SPEED);
-    while(t.read_ms() < amt);
     t.stop();
     oled_2 = 0;
     pi.stop();
@@ -443,8 +487,10 @@
     oled_3 = 1;
     pi.locate(0,0);
     pi.printf("Back %d", amt);
-    pi.backward(DRIVE_SPEED);
     t.start();
+    pi.backward(.5*DRIVE_SPEED);
+    while(t.read_ms() < (amt-500));
+    pi.backward(.5*DRIVE_SPEED);
     while(t.read_ms() < amt);
     t.stop();
     oled_3 = 0;
@@ -460,7 +506,7 @@
     pi.printf("Right %f", deg);
     pi.right(TURN_SPEED);
     t.start();
-    while(t.read_ms() < (deg/360)*TIME_FACT);
+    while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
     t.stop();
     oled_4 = 0;
     pi.stop();
@@ -484,7 +530,7 @@
     pi.printf("Left %f", deg);
     pi.left(TURN_SPEED);
     t.start();
-    while(t.read_ms() < (deg/360)*TIME_FACT);
+    while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
     t.stop();
     oled_4 = 0;
     oled_2 = 0;

--- a/main.h	Sat Dec 06 22:04:53 2014 +0000
+++ b/main.h	Sun Dec 07 04:39:33 2014 +0000
@@ -21,7 +21,7 @@
 #define DRIVE_RATE  1/50
 #define TIME_FACT 1820
 #define CAL_SPEED .1
-#define CLOSE_ENOUGH .02
+#define CLOSE_ENOUGH .0008
 
 
 /**