Diff: main.c

Revision:

34:3066686d5152

Parent:

32:8b589710632b

Child:

35:a1c14c6d9282

--- a/main.c	Tue Dec 09 23:07:08 2014 +0000
+++ b/main.c	Wed Dec 10 18:49:45 2014 +0000
@@ -40,10 +40,11 @@
     pi.locate(0,1);
     pi.printf("%f mV", pi.battery());
     pi.sensor_auto_calibrate();
+    timerWait(.2);
 
     do {
         pos = pi.line_position();
-        if(pos > over_thresh) {
+        if(pos > over_thresh) { 
             pi.right_motor(CAL_SPEED);
             pi.left_motor(CAL_SPEED-correction);
         } else if(pos < -over_thresh) {
@@ -61,7 +62,8 @@
     if(pos != -1) {
         timer.stop();
     } else {
-        return 1;
+        oled_1 = 1;
+        while(1);
     }
     left(180);
     Timer caltimer;
@@ -83,26 +85,27 @@
         pi.printf("Pos: %f", pos);
     } while(pos != -1 && pos <= 0.3);
     caltimer.stop();
-    cal_time = caltimer.read_ms();
+    cal_time = caltimer.read_ms()*2;
     pi.stop();
     if(pos != -1) {
         timer.stop();
     } else {
-        return 1;
+        oled_1 = 1;
+        while(1);
     }
     right(180);
     timerWait(.2);
     
     while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
-        pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
+        pi.right((pos < 0 ? -.6*.5*CAL_SPEED : .6*.5*CAL_SPEED));
         pi.cls();
         pi.locate(0,0);
         pi.printf("O: %f", pos);
     }
     pi.stop();    
     timerWait(0.2);
-    backward(500);
-
+    backward(400);
+    
     // ---------------------------------------------------------
     // DONE calibrating. Begin reading in coordinate
     // file and run robot. 
@@ -134,7 +137,7 @@
         pi.cls();
         pi.locate(0,0);
         pi.printf("sscanf1");
-        return 1;
+        while(1);
     }
     cur = strchr(instbuf, '\n');
     cur++;
@@ -168,10 +171,23 @@
         
         delta_x = x-last_x;
         delta_y = y-last_y;
-
-        /* Compute turn angle and turn. */
-        theta = atan(((double) delta_y)/((double) delta_x));
-        theta *= 57.29;
+        if (delta_y == 0) {
+            if (delta_x < 0) {
+                theta = 180;
+            } else {
+                theta = 0;
+            }
+        } 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;
         }
@@ -209,9 +225,8 @@
         }
         pi.cls();
         pi.locate(0,0);
-        pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
-        forward(0.4*cal_time*(dist/(double)dim_x));
-
+        pi.printf("d:%f", 0.55*cal_time*(dist/(double)dim_x));
+        forward(0.55*cal_time*(dist/(double)dim_x));
         last_x = x;
         last_y = y;
         next = strchr(cur, '\n');
@@ -228,7 +243,8 @@
     pi.cls();
     pi.locate(0,0);
     pi.printf("Done");
-    pi.stop(); // Just in case. Prevent runaway polio. That's all bad!
+    pi.stop(); 
+    oled_3 = 0;
     while (1);
 }
 
@@ -263,12 +279,25 @@
 void forward(int amt)
 {
     Timer t;
+    float ms = 0;
+    float last_ms = 0;
     pi.locate(0,0);
     pi.printf("Fwd %d", amt);
     t.start();
-    pi.left_motor(DRIVE_SPEED+.0023);
+    pi.left_motor(DRIVE_SPEED+.002);
     pi.right_motor(DRIVE_SPEED);
-    while(t.read_ms() < amt);
+    ms = t.read_ms();
+    while(ms < amt) {
+        if (ms-last_ms > 1000) {
+            t.stop();
+            right(3);
+            last_ms = ms;
+            t.start();
+            pi.left_motor(DRIVE_SPEED+.002);
+            pi.right_motor(DRIVE_SPEED);
+        }
+        ms = t.read_ms();
+    }
     pi.stop();
 }
 
@@ -286,6 +315,7 @@
     if(deg < 0) {
         return left(-1*deg);
     }
+    deg = deg+1.5;
     Timer t;
     float amt = ((float)(deg/360)*TIME_FACT);
     t.start();
@@ -306,6 +336,7 @@
     if(deg < 0) {
         return right(-1*deg);
     }
+    deg = deg + 1.5;
     Timer t;
     pi.left(TURN_SPEED);
     t.start();