Diff: main.cpp

Revision:

1:ac7fc0b5f878

Parent:

0:80d9725a7342

Child:

2:e513ada0ecf1

--- a/main.cpp	Tue May 20 13:00:55 2014 +0000
+++ b/main.cpp	Wed May 21 11:31:57 2014 +0000
@@ -4,98 +4,84 @@
 DigitalOut myled(LED1);
 m3pi huey;
 
-int main() {
+//stops without hitting
+void slowStop(float speed)
+{
+    float i;
+    for(i = speed; i > 0; i = i - 0.05)
+    {
+        huey.forward(i);
+        wait(0.075);      
+    }
+    huey.stop();
+}
+
+void printBattery()
+{
     //prints battery voltage on led screen
     float bat = huey.battery();
-    huey.printf("%f",bat);
-    
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.5);
-    
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.5);
+    huey.printf("Bat volt");
+    huey.locate(0,1);
+    huey.printf("%f",bat); 
+}
+
+void smoothFollow(float position, float speed)
+{
+    float u = speed/4;
+    u = u * position;
+    if(speed+u > 1)
+    {
+        huey.stop();
+        huey.printf("Fast Er");
+    }
+    else
+    {
+        huey.right_motor(speed+u);
+        huey.left_motor(speed-u);
+    }
     
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.5);
-    
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.5);
-    
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.5);
-    
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.1);
-    huey.forward(0.1);
-    wait(0.1);
-    huey.backward(0.1);
-    wait(0.5);
+}
+
+void follow(float position, float speed)
+{
+    float tol = 0.05;
+    if(position > tol)
+    {
+        huey.right(speed);     
+    }
+    else if(position < (-tol))
+    {
+        huey.left(speed);    
+    }
+    else
+    {
+        huey.forward(speed);
+    }
+        
+}
+
+int main() {
+    //calibrates sensors
+    huey.sensor_auto_calibrate();  
+
+    //printBattery();
     
     
+    //huey.printf("%f", huey.line_position());
     
-    huey.stop();
+    float speed = 0.1;
+    float pos;
+    int z=1;
+    while(z==1)
+    {
+        //huey.right_motor(speed);
+        pos = huey.line_position();
+        huey.printf("%f",pos );
+        smoothFollow(pos, speed);
+
+    }
+
+    
     
     
     
@@ -108,3 +94,4 @@
         wait(0.2);
     }
 }
+