Diff: main.cpp

Revision:

7:cb6947e1f1d3

Parent:

6:be710baf53ec

Child:

9:8e3392380915

--- a/main.cpp	Thu Jan 22 13:44:59 2015 +0000
+++ b/main.cpp	Thu Feb 05 12:37:47 2015 +0000
@@ -26,20 +26,47 @@
 bool WaitUntilButtonPressed();
 
 // Eigene Funktionsdefinitionen hier
-
-
-    
+bool FrontIsClearU()
+{
+    int dist = 0;
+        usensor.start();
+        wait_ms(10);
+        dist=usensor.get_dist_cm();
+    if(dist < 5)
+        return false;
+    else
+        return true;
+        
+//        pc.printf("\r\nCount =%d",count);
+//        pc.printf("Distance: %d",dist );
+}    
 // *************  Hauptprogramm ************
 int main()
 {
-    // while(!Start()) {}   // wait to start
-    wait_ms(200);
+    int dist = 0;
+    unsigned char count=0;
+/*    while(count < 20) {
+        usensor.start();
+        wait_ms(500);
+        dist=usensor.get_dist_cm();
+        pc.printf("\r\nCount =%d",count);
+        pc.printf("Distance: %d",dist );
+        count++;
 
-    Move();   
+    }
+*/    // while(!Start()) {}   // wait to start
+    wait_ms(200);
+    while(FrontIsClearU())
+    {
+        MoveTicks(40);
+    }
     TurnLeft();
-    Move();
+    while(FrontIsClearU())
+    {
+        MoveTicks(40);
+    }
 
-    TurnLeftTicks(2000);    // Anzahl der MotorSensorwerte 
+    // TurnLeftTicks(2000);    // Anzahl der MotorSensorwerte
 
     ShutOff();
     return 0;