Revision 20:76718145b403, committed 2014-12-02

Comitter:

chstrchu

Date:

Tue Dec 02 03:42:32 2014 +0000

Parent:

19:47759cf4f9b9

Child:

21:0c80a5d89ea3

Commit message:

This afternoon's changes

Changed in this revision

--- a/main.c	Mon Dec 01 20:16:23 2014 +0000
+++ b/main.c	Tue Dec 02 03:42:32 2014 +0000
@@ -79,11 +79,9 @@
             pi.right_motor(-DRIVE_SPEED);
             pi.left_motor(-DRIVE_SPEED);
         }
-        /*
         pi.cls();
         pi.locate(0,0);
         pi.printf("P: %f", pos);
-        */
     } while (pos != -1 && pos <= 0.3);
     pi.stop();
     wait(1);
@@ -98,7 +96,6 @@
     }
 
     wait(1);
-    
     do {
         pos = pi.line_position();
         if(pos > over_thresh) {
@@ -150,6 +147,8 @@
         oled_1 = 1;
         timer.stop();
         cal_time = timer.read();
+        pi.cls();
+        pi.locate(0,0);
         pi.printf("T: %d", timer.read_ms());
     } else {
         pi.cls();
@@ -157,7 +156,7 @@
         pi.printf("lP:%f", pos);
         return 1;
     }
-
+    /*
     while(pi.line_position() == 1);
     do {
         pos = pi.line_position();
@@ -185,6 +184,7 @@
         pi.printf("LP:%f", pos);
         while(1);
     }
+    */
     // If we got here, calibration is complete.
     
     //
@@ -202,24 +202,45 @@
     last_x = 0;
     last_y = 0;
     ps_file = fopen("/local/test.ps", "r");
+    if (ps_file == NULL) {
+        return 1;
+    }
+    pi.cls();
+    pi.locate(0,0);
+    pi.printf("open");
     /* PS parsing loop. */
     memset(instbuf, 0, instbuflen);
-    bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file);
+    bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
+    pi.cls();
+    pi.locate(0,0);
+    pi.printf("%.7s", instbuf);
     err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
     if (err != 2) {
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("sscanf1");
         return 1;
     }
     cur = strchr(instbuf, '\n');
+    cur++;
+    offset = instbuf+instbuflen-cur;
+    memcpy(instbuf, cur, offset);
     while (1) {
         memset(instbuf+offset, 0, instbuflen-offset);
         bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
         if (bytes_read == 0) {
+            pi.cls();
+            pi.locate(0,0);
+            pi.printf("bytes0");
             break;
         }
         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;
@@ -229,34 +250,45 @@
             theta = tan((double) delta_x/delta_y);
             theta *= 57.2957795;
             delta_a = theta-angle;
+            pi.cls();
+            pi.locate(0,0);
+            pi.printf("%d", delta_a);
             if (delta_a > 0) {
-                pi.left(TURN_SPEED);
+                left(delta_a);
             } else {
-                pi.right(TURN_SPEED);
+                right(delta_a);
             }
-            wait(0.5*(delta_a/360));
-
+          
             /* 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.forward(DRIVE_SPEED);
-            wait(cal_time*(dist/dim_x));
+            // forward(cal_time*(dist/dim_x));
+            forward(5000);
+            // wait(cal_time*(dist/dim_x));
+            // pi.stop();
 
             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;
+            pi.cls();
+            pi.locate(0,0);
+            pi.printf("waiting!");
         }
         offset = instbuf+instbuflen-cur;
         memcpy(instbuf, cur, offset);
@@ -265,6 +297,9 @@
     // We should never reach this point!
     //
     // Yes, we should...
+    pi.cls();
+    pi.locate(0,0);
+    pi.printf("done");
     return 0;
 }
 
@@ -274,17 +309,23 @@
     char *srch;
     matches = sscanf(buf, "%d %d", x, y);
     if (matches != 2) {
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("nomatch");
         return 0;
     }
     srch = strchr(buf, ' ');
     srch++;
-    srch = strchr(buf, ' ');
+    srch = strchr(srch, ' ');
     srch++;
     if (srch[0] == 'm') {
         *draw = 0;
-    } else if (srch[0] == 'd') {
+    } else if (srch[0] == 'l') {
         *draw = 1;
     } else {
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("%.8s", srch);
         return 0;
     }
     return 1;
@@ -298,7 +339,7 @@
     pi.locate(0,0);
     pi.printf("Fwd %d", amt);
     pi.forward(DRIVE_SPEED);
-    while(t.read_ms() < amt*DRIVE_RATE*1000);
+    while(t.read_ms() < amt);
     t.stop();
     oled_2 = 0;
     pi.stop();
@@ -313,7 +354,7 @@
     pi.printf("Back %d", amt);
     pi.backward(DRIVE_SPEED);
     t.start();
-    while(t.read_ms() < amt*DRIVE_RATE*1000);
+    while(t.read_ms() < amt);
     t.stop();
     oled_3 = 0;
     pi.stop();