Diff: source/Positioning.cpp

Revision:

71:ddf4eb5c3081

Parent:

70:922cbbfebf02

Child:

72:4e8a151d804e

--- a/source/Positioning.cpp	Thu Apr 20 07:36:50 2017 +0000
+++ b/source/Positioning.cpp	Thu Apr 20 11:37:24 2017 +0000
@@ -69,9 +69,9 @@
 void positioning()
 {
     printf("positioning...\r\n");
-    
-    
-    
+
+
+
     clamp_heading();
 }
 
@@ -80,7 +80,7 @@
 int initial_positioning()
 {
     if(init == false) {
-
+        printf("initial_positioning init\r\n");
         last_dist_r = 100;
         last_dist_l = 100;
 
@@ -96,8 +96,7 @@
     } else {
 
         if(t2 > 0.2f) {
-            t2 = 0;
-
+            t2.reset();
             float dist_r = getDistanceIR(4);
             float dist_l = getDistanceIR(0);
 
@@ -154,7 +153,7 @@
                 printf("heading: %f \r\n", current_heading);
 
                 //finished
-                return 11;
+                return 17;
 
             }
         }
@@ -163,38 +162,4 @@
     return 16;
 }
 
-/*
-turn_straight_right();
-turn_straight_left();
 
-while(last_dist_r > sensors[r]) {
-    turn_sensor_right(1); //turn sensor + 1 deg
-    wait(0.1f)
-    deg_r += 1;
-    last_dist_r = sensors[r];
-}
-
-while(last_dist_l > sensors[l]) {
-    turn_sensor_left(-1); //turn sensor - 1 deg
-    wait(0.1f)
-    deg_l += 1;
-    last_dist_l = sensors[l];
-}
-
-int deg_l_2=0;
-turn_straight_left();
-last_dist_l = 0;
-
-while(last_dist_l < sensors[l]) {
-    turn_sensor_left(1); //turn sensor +1 deg (positiv = uhrzeigersinn)
-    wait(0.1f);
-    deg_l_2 += 1;
-    last_dist_l = sensors[l];
-}
-
-turn_straight_right();
-turn_straight_left();
-
-wait(0.2f);
-*/
-