Diff: main.c

Revision:

12:1aa6b8a74136

Parent:

11:a30f30d3066e

Child:

13:070846d87d4a

--- a/main.c	Sun Nov 16 05:26:24 2014 +0000
+++ b/main.c	Sun Nov 16 09:11:34 2014 +0000
@@ -14,6 +14,7 @@
  * These are global data Used externally in all other files 
  */
 m3pi        pi;
+Timer       timer;
 
 //
 // Digital inputs to the mBed
@@ -45,58 +46,115 @@
     //
     // Basic setup information
     start_button.mode(PullUp);
+        
+    pi.sensor_auto_calibrate();
+    pi.backward(DRIVE_SPEED);
+    float pos;
+    
+    do {
+        pos = pi.line_position();
+        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);
+    } else {
+        pi.stop();
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("LP: %f",pos);
+        while(1);
+    }
 
+    while(pi.line_position() == 1);
+    do {
+        pos = pi.line_position();
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("Pos: %f", pos);
+    } while(pos != -1 && pos != 1);
+    if(pos == 1) {
+        oled_1 = 1;
+        timer.stop();
+        pi.stop();           
+    } else {
+        pi.stop();
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("LP:%f", pos);
+        while(1);
+    }
+    
     //
     // Main program loop.
-    robot_loop();
+    // robot_loop();
     
     //
     // We should never reach this point!
+    while(1);
 }
 
 int forward(int amt)
 {
+    Timer t;
+    t.start();
     oled_2 = 1;
     pi.locate(0,0);
     pi.printf("Fwd %d", amt);
     pi.forward(DRIVE_SPEED);
-    wait(amt*DRIVE_RATE);
+    while(t.read_ms() < amt*DRIVE_RATE*1000);
+    t.stop();
     oled_2 = 0;
+    pi.stop();
     return EXIT_SUCCESS;
 }
 
 int backward(int amt)
 {
+    Timer t;
     oled_3 = 1;
     pi.locate(0,0);
     pi.printf("Back %d", amt);
     pi.backward(DRIVE_SPEED);
-    wait(amt*DRIVE_RATE);
+    t.start();
+    while(t.read_ms() < amt*DRIVE_RATE*1000);
+    t.stop();
     oled_3 = 0;
+    pi.stop();
     return EXIT_SUCCESS;
 }
 
 int right(float deg)
 {
+    Timer t;
     oled_4 = 1;
     pi.locate(0,0);
     pi.printf("Right %f", deg);
     pi.right(TURN_SPEED);
-    wait(deg/360);
+    t.start();
+    while(t.read_ms() < (deg/360)*1000);
+    t.stop();
     oled_4 = 0;
+    pi.stop();
     return EXIT_SUCCESS;
 }
 
 int left(float deg)
 {
+    Timer t;
     oled_4 = 1;
     oled_2 = 1;
     pi.locate(0,0);
     pi.printf("Left %f", deg);
     pi.left(TURN_SPEED);
-    wait(deg/360);
+    t.start();
+    while(t.read_ms() < (deg/360)*1000);
+    t.stop();
     oled_4 = 0;
     oled_2 = 0;
+    pi.stop();
     return EXIT_SUCCESS;
 }