Diff: source/Movement.cpp

Revision:

76:bdbdd64cdd80

Parent:

74:d9c387b83196

Parent:

75:dba260cb5ae4

Child:

77:ff87a10c4baf

--- a/source/Movement.cpp	Tue Apr 25 07:59:59 2017 +0000
+++ b/source/Movement.cpp	Tue Apr 25 08:02:22 2017 +0000
@@ -9,7 +9,6 @@
 bool is_turning = false;
 bool direction = false;
 float wanted_deg = 0;
-float current_deg = 0;
 Timer t;
 float previous_t = 0;
 bool first_search_cycle = true;                                                 // flag for state first time in function "move in search for brick" 
@@ -34,17 +33,16 @@
     return 0;
 }
 
-void turn_for_deg(float deg)
+float turn_for_deg(float deg) //if deg not 0 equals initilisation.
 {
 
-    if(is_turning == false) {
+    if(deg != 0) {
 
         is_turning = true;
         float left = 0;
         float right = 0;
 
-        wanted_deg = deg;
-        current_deg = 0;
+        wanted_deg = sqrt(deg*deg);
 
         if(deg < 0) { // turn left
             direction = 1;
@@ -57,21 +55,20 @@
         }
         set_speed(left, right);
         t.reset();
-        previous_t = 0;
         t.start();
 
     } else {
 
         float speed_left = get_speed_left();
-        float delta_t = t - previous_t;
-        previous_t = t;
-        current_deg +=  360.0f / (2*radius*M_PI) * delta_t * speed_left;
-        if(current_deg * current_deg > wanted_deg * wanted_deg) {
+        wanted_deg -=  360.0f / (2*radius*M_PI) * t.read() * fabsf(speed_left);
+        t.reset();
+        if(wanted_deg <= 0) {
             set_speed(0,0);
             is_turning = false;
             t.stop();
         }
     }
+    return (wanted_deg);
 }