Diff: source/Movement.cpp

Revision:

75:dba260cb5ae4

Parent:

71:ddf4eb5c3081

Child:

76:bdbdd64cdd80

--- a/source/Movement.cpp	Fri Apr 21 12:37:07 2017 +0000
+++ b/source/Movement.cpp	Mon Apr 24 15:01:23 2017 +0000
@@ -8,7 +8,6 @@
 bool is_turning = false;
 bool direction = false;
 float wanted_deg = 0;
-float current_deg = 0;
 Timer t;
 float previous_t = 0;
 
@@ -30,17 +29,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;
@@ -53,21 +51,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);
 }