Diff: main.c

Revision:

16:f1beef7beeb9

Parent:

14:41fa8b95a9ab

Parent:

15:14d4e7021125

Child:

17:c72c092fcdf7

--- a/main.c	Fri Nov 28 21:08:55 2014 +0000
+++ b/main.c	Sun Nov 30 21:52:58 2014 +0000
@@ -37,6 +37,8 @@
 DigitalOut  oled_3(LED3);
 DigitalOut  oled_4(LED4);
 
+/* Local File System */
+LocalFileSystem local("local");
 
 /**
  * @brief Entry point. Main loop.
@@ -50,35 +52,101 @@
     //
     // Drawing environment calibration.
     pi.sensor_auto_calibrate();
-    pi.backward(DRIVE_SPEED);
-    float   pos;
-    float   over_thresh = 0.5;
-    float   correction  = 0.1;
+    //pi.backward(DRIVE_SPEED);
+    float   pos         = 0;
+    float   over_thresh = 0.05;
+    float   correction  = 0.2*DRIVE_SPEED;
+
+    wait(1);
+
+    do {
+        pos = pi.line_position();
+        if(pos > over_thresh) {
+            pi.right_motor(-DRIVE_SPEED);
+            pi.left_motor(-DRIVE_SPEED+correction);
+        } else if(pos < -over_thresh) {
+            pi.left_motor(-DRIVE_SPEED);
+            pi.right_motor(-DRIVE_SPEED+correction);
+        } else {
+            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.2);
+    pi.stop();
+    wait(1);
+    if (pos != -1) {
+        timer.start();
+        pi.forward(DRIVE_SPEED);
+    } else {
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("LP: %f",pos);
+        return 1;
+    }
+
+    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(DRIVE_SPEED-correction);
         } else if(pos < -over_thresh) {
             pi.left_motor(DRIVE_SPEED);
-            pi.right_motor(DRIVE_SPEED - correction);
+            pi.right_motor(DRIVE_SPEED-correction);
         } else {
-            pi.forward(DRIVE_SPEED);
+            pi.right_motor(DRIVE_SPEED);
+            pi.left_motor(DRIVE_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;
+    }
+    pi.right(TURN_SPEED);
+    wait(0.5);
+    pi.stop();
+    do {
+        pos = pi.line_position();
+        if(pos > over_thresh) {
+            pi.right_motor(-DRIVE_SPEED);
+            pi.left_motor(-DRIVE_SPEED+correction);
+        } else if(pos < -over_thresh) {
+            pi.left_motor(-DRIVE_SPEED);
+            pi.right_motor(-DRIVE_SPEED+correction);
+        } else {
+            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 != 1);
-    if(pos == 1) {
-        timer.start();
-        pi.forward(DRIVE_SPEED);
+    } 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.stop();
         pi.cls();
         pi.locate(0,0);
-        pi.printf("LP: %f",pos);
-        while(1);
+        pi.printf("lP:%f", pos);
+        return 1;
     }
 
     while(pi.line_position() == 1);
@@ -114,9 +182,14 @@
     // Main program loop.
     // robot_loop();
     
+    ps_file = fopen("/local/test.ps", "r");
+    while () {
+        memset(instbuf, 0, instbuflen);
+        fread(instbuf, sizeof(char), instbuflen, ps_file);
+    }
     //
     // We should never reach this point!
-    while(1);
+    return 0;
 }
 
 int forward(int amt)