Revision 21:0c80a5d89ea3, committed 2014-12-06

Comitter:

lsaristo

Date:

Sat Dec 06 22:04:53 2014 +0000

Parent:

20:76718145b403

Child:

22:46b9d9b2e35c

Commit message:

Worked on calibrating robot movements. Accurate enough for now

Changed in this revision

--- a/main.c	Tue Dec 02 03:42:32 2014 +0000
+++ b/main.c	Sat Dec 06 22:04:53 2014 +0000
@@ -56,28 +56,57 @@
     // Basic setup information
     start_button.mode(PullUp);
         
+    start:
+    pi.cls();
+    pi.locate(0,0);
+    pi.printf("PiCO");
+    pi.locate(0,1);
+    pi.printf("%f mV", pi.battery());
+    wait(.5);
+    /* while(start_button) {
+        oled_2 = 1;
+        wait(.5);
+        pi.locate(0,0);
+        pi.printf("Ready");
+        oled_2 = 0;
+    } /**/
+    pi.cls();
+    pi.locate(0,0);
+    // pi.printf("GO!");
+    wait(.5);
+
+    /*
+    while(1) {
+        if(!start_button) {
+            pi.stop();
+            goto start;
+        } else {
+            goto calibrate;    
+        }
+    }
+    /**/
+        
     //
     // Drawing environment calibration.
+    int cal_count = 1;
     pi.sensor_auto_calibrate();
     //pi.backward(DRIVE_SPEED);
     float   pos         = 0;
     float   over_thresh = 0.05;
     float   correction  = 0.2*DRIVE_SPEED;
     float   cal_time;
-    
     wait(1);
-
+    /*
     do {
         pos = pi.line_position();
         if(pos > over_thresh) {
-            pi.right_motor(-DRIVE_SPEED);
-            pi.left_motor(-DRIVE_SPEED+correction);
+            pi.left_motor(-CAL_SPEED);
+            pi.right_motor(-CAL_SPEED+correction);
         } else if(pos < -over_thresh) {
-            pi.left_motor(-DRIVE_SPEED);
-            pi.right_motor(-DRIVE_SPEED+correction);
+            pi.right_motor(-CAL_SPEED);
+            pi.left_motor(-CAL_SPEED+correction);
         } else {
-            pi.right_motor(-DRIVE_SPEED);
-            pi.left_motor(-DRIVE_SPEED);
+            pi.backward(CAL_SPEED);
         }
         pi.cls();
         pi.locate(0,0);
@@ -85,28 +114,62 @@
     } while (pos != -1 && pos <= 0.3);
     pi.stop();
     wait(1);
+    
     if (pos != -1) {
         timer.start();
-        pi.forward(DRIVE_SPEED);
+        pi.forward(CAL_SPEED);
     } else {
         pi.cls();
         pi.locate(0,0);
         pi.printf("LP: %f",pos);
+        forward(1000);
+        goto calibrate;
         return 1;
     }
-
+    /**/
+    
+    // wait(1);
+    
+    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) {
+        oled_1 = 1;
+        timer.stop();
+        pi.printf("T: %d", timer.read_ms());
+    } else {
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("lP:%f", pos);
+        return 1;
+    }
+    right(180);
     wait(1);
     do {
         pos = pi.line_position();
         if(pos > over_thresh) {
-            pi.right_motor(DRIVE_SPEED);
-            pi.left_motor(DRIVE_SPEED-correction);
+            pi.right_motor(CAL_SPEED);
+            pi.left_motor(CAL_SPEED-correction);
         } else if(pos < -over_thresh) {
-            pi.left_motor(DRIVE_SPEED);
-            pi.right_motor(DRIVE_SPEED-correction);
+            pi.left_motor(CAL_SPEED);
+            pi.right_motor(CAL_SPEED-correction);
         } else {
-            pi.right_motor(DRIVE_SPEED);
-            pi.left_motor(DRIVE_SPEED);
+            pi.right_motor(CAL_SPEED);
+            pi.left_motor(CAL_SPEED);
         }
         pi.cls();
         pi.locate(0,0);
@@ -123,9 +186,24 @@
         pi.printf("lP:%f", pos);
         return 1;
     }
-    pi.right(TURN_SPEED);
-    wait(0.5);
-    pi.stop();
+    right(180);
+    timerWait(.07);
+    pos = pi.line_position();
+    while(fabs(pos) > CLOSE_ENOUGH) {
+        pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
+        timerWait(.08);
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("O: %f", pos);
+        pi.stop();
+        pos = pi.line_position();
+        timerWait(.2);
+    }
+    timerWait(2);
+    
+    //
+    // Pivot 180 degrees to go to the starting position.
+    /*
     do {
         pos = pi.line_position();
         if(pos > over_thresh) {
@@ -135,8 +213,7 @@
             pi.left_motor(-DRIVE_SPEED);
             pi.right_motor(-DRIVE_SPEED+correction);
         } else {
-            pi.right_motor(-DRIVE_SPEED);
-            pi.left_motor(-DRIVE_SPEED);
+            pi.backward(DRIVE_SPEED);
         }
         pi.cls();
         pi.locate(0,0);
@@ -156,6 +233,7 @@
         pi.printf("lP:%f", pos);
         return 1;
     }
+    /**/
     /*
     while(pi.line_position() == 1);
     do {
@@ -271,10 +349,19 @@
             if (dist < 0) {
                 dist *= -1;
             }
-            // forward(cal_time*(dist/dim_x));
-            forward(5000);
-            // wait(cal_time*(dist/dim_x));
-            // pi.stop();
+            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);
 
             last_x = x;
             last_y = y;
@@ -338,7 +425,11 @@
     oled_2 = 1;
     pi.locate(0,0);
     pi.printf("Fwd %d", amt);
-    pi.forward(DRIVE_SPEED);
+    pi.left_motor(DRIVE_SPEED+.0023);
+    pi.right_motor(DRIVE_SPEED);
+    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;
@@ -369,13 +460,21 @@
     pi.printf("Right %f", deg);
     pi.right(TURN_SPEED);
     t.start();
-    while(t.read_ms() < (deg/360)*1000);
+    while(t.read_ms() < (deg/360)*TIME_FACT);
     t.stop();
     oled_4 = 0;
     pi.stop();
     return EXIT_SUCCESS;
 }
 
+void timerWait(float seconds)
+{
+    Timer t;
+    t.start();
+    while(t.read_us() < 1000000*seconds);
+    t.stop();
+}
+
 int left(float deg)
 {
     Timer t;
@@ -385,7 +484,7 @@
     pi.printf("Left %f", deg);
     pi.left(TURN_SPEED);
     t.start();
-    while(t.read_ms() < (deg/360)*1000);
+    while(t.read_ms() < (deg/360)*TIME_FACT);
     t.stop();
     oled_4 = 0;
     oled_2 = 0;

--- a/main.h	Tue Dec 02 03:42:32 2014 +0000
+++ b/main.h	Sat Dec 06 22:04:53 2014 +0000
@@ -13,11 +13,15 @@
 #include <string.h>
 #include <stdarg.h>
 #include <stdio.h>
-#define TURN_SPEED  0.25
-#define DRIVE_SPEED 0.1
+#include <math.h>
+#define TURN_SPEED  0.15
+#define DRIVE_SPEED 0.2
 #define ERR_SUCCESS 0
 #define ERR_FAILURE 1
 #define DRIVE_RATE  1/50
+#define TIME_FACT 1820
+#define CAL_SPEED .1
+#define CLOSE_ENOUGH .02
 
 
 /**
@@ -92,4 +96,12 @@
  * Decide what to do next based on the status of the drawing so far.
  */
 void next_action();
+
+/**
+ * @brief Wait for a number of seconds, possibly fractional.
+ *
+ * This is because mBed wait() function SUCKS
+ */
+void timerWait(float);
+
 #endif