Diff: main.cpp

Revision:

3:6f773c2ba3a0

Parent:

2:e513ada0ecf1

Child:

4:14b00a82d206

--- a/main.cpp	Wed May 21 11:52:59 2014 +0000
+++ b/main.cpp	Wed May 21 13:54:40 2014 +0000
@@ -1,20 +1,11 @@
 #include "mbed.h"
 #include "m3pi_ng.h"
+#include "DigitalIn.h"
 
 DigitalOut myled(LED1);
 m3pi huey;
+DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
 
-//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()
 {
@@ -42,33 +33,46 @@
     
 }
 
-void follow(float position, float speed)
+void slowStop(float speed, float waitTime, int steps)
 {
-    float tol = 0.05;
-    if(position > tol)
+    float i;
+    for(i = speed; i > 0; i = i - (speed/steps))
     {
-        huey.right(speed);     
+        smoothFollow(huey.line_position(),i);
+        wait(waitTime);      
     }
-    else if(position < (-tol))
-    {
-        huey.left(speed);    
-    }
-    else
-    {
-        huey.forward(speed);
-    }
-        
+    huey.stop();
 }
 
+
+int pushToStart(void)
+{
+    DigitalIn button(p21);
+    int value = button.read();
+    huey.printf("%d", value);
+    return value;
+}
+
+
+/////////////////////////////////////////////////////////////////
 int main() {
+    
+    m3pi_IN[0].mode(PullUp);
+    m3pi_IN[1].mode(PullUp);
+    
+    int start;
+    do 
+    {
+        start = pushToStart();  
+    }while(start == 1);
+    
+    wait(1);
     //calibrates sensors
     huey.sensor_auto_calibrate();  
 
-    //printBattery();
+    printBattery();
     
     
-    //huey.printf("%f", huey.line_position());
-    
     float speed = 0.25;
     float pos;
     int z=1;
@@ -76,8 +80,16 @@
     {
         //huey.right_motor(speed);
         pos = huey.line_position();
-        huey.printf("%f",pos );
         smoothFollow(pos, speed);
+        if(m3pi_IN[0]==0)
+        {
+            slowStop(speed, 0.05, 3);
+            huey.stop();
+            while(m3pi_IN[0]==0)
+            {
+                huey.printf("Stuck");    
+            }
+        }
 
     }