Diff: main.c

Revision:

32:8b589710632b

Parent:

31:1e950ee04481

Child:

34:3066686d5152

--- a/main.c	Tue Dec 09 21:18:07 2014 +0000
+++ b/main.c	Tue Dec 09 22:56:54 2014 +0000
@@ -34,15 +34,12 @@
     int     instbuflen  = 250;
     char    instbuf[instbuflen];
  
-    start_button.mode(PullUp);
+    // ---------------------------------------------------------
+    // First calibrate robot and figure out space dimensions.
     pi.cls();
-    pi.locate(0,0);
-    pi.printf("Ready");
     pi.locate(0,1);
     pi.printf("%f mV", pi.battery());
     pi.sensor_auto_calibrate();
-    
-    wait(1);
 
     do {
         pos = pi.line_position();
@@ -67,7 +64,6 @@
         return 1;
     }
     left(180);
-    
     Timer caltimer;
     caltimer.start();
     do {
@@ -95,7 +91,8 @@
         return 1;
     }
     right(180);
-    timerWait(.5);
+    timerWait(.2);
+    
     while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
         pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
         pi.cls();
@@ -103,23 +100,12 @@
         pi.printf("O: %f", pos);
     }
     pi.stop();    
-    timerWait(0.5);
+    timerWait(0.2);
     backward(500);
-    
-    /* Maybe uncomment this depending on the surface.
-    timerWait(.5);
-    while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
-        pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("O: %f", pos);
-    }
-    pi.stop();
-    */
-    
-    timerWait(.6);
 
-    // Main program loop.
+    // ---------------------------------------------------------
+    // 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;
@@ -154,8 +140,12 @@
     cur++;
     offset = instbuf+instbuflen-cur-1;
     memcpy(instbuf, cur, offset);
+    
+    
+    // ---------------------------------------------------------
+    // File open and buffer setup. Begin reading instructions and
+    // moving robot. Refill buffer after each instruction. 
     while (1) {
-        timerWait(.18);
         memset(instbuf+offset, 0, instbuflen-offset);
         bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
         
@@ -168,12 +158,14 @@
         
         cur = instbuf;
         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;
 
@@ -184,13 +176,14 @@
             theta += 180;
         }
         delta_a = theta-angle;
+        
         if (delta_x < 0 && delta_y < 0) {
             delta_a += 180;
         }
+        
         if (delta_a > 180) {
             delta_a -= 360;
-        }
-        if (delta_a < -180) {
+        } else if (delta_a < -180) {
             delta_a = 360 + delta_a;
         }
         angle += delta_a;
@@ -200,7 +193,6 @@
         pi.cls();
         pi.locate(0,0);
         pi.printf("a:%f", delta_a);
-        timerWait(1.5);
         left(delta_a);
 
         /* Put pen into position. */
@@ -218,7 +210,6 @@
         pi.cls();
         pi.locate(0,0);
         pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
-        timerWait(1.5);
         forward(0.4*cal_time*(dist/(double)dim_x));
 
         last_x = x;
@@ -237,6 +228,7 @@
     pi.cls();
     pi.locate(0,0);
     pi.printf("Done");
+    pi.stop(); // Just in case. Prevent runaway polio. That's all bad!
     while (1);
 }
 
@@ -277,7 +269,6 @@
     pi.left_motor(DRIVE_SPEED+.0023);
     pi.right_motor(DRIVE_SPEED);
     while(t.read_ms() < amt);
-    t.stop();
     pi.stop();
 }
 
@@ -286,10 +277,7 @@
     Timer t;
     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();
     pi.stop();
 }
 
@@ -303,7 +291,6 @@
     t.start();
     pi.right(TURN_SPEED);
     while(t.read_ms() < amt);
-    t.stop();
     pi.stop();
 }
 
@@ -312,7 +299,6 @@
     Timer t;
     t.start();
     while(t.read_us() < 1000000*seconds);
-    t.stop();
 }
 
 void left(float deg)
@@ -324,16 +310,5 @@
     pi.left(TURN_SPEED);
     t.start();
     while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
-    t.stop();
     pi.stop();
-}
-
-void pen_down()
-{
-    oled_1 = 1;
-}
-
-void pen_up()
-{
-    oled_1 = 0;
 }
\ No newline at end of file