Diff: source/Movement.cpp

Revision:

62:628f8a4e857c

Parent:

55:36c290715769

Child:

65:b27aa01c2cf6

--- a/source/Movement.cpp	Tue Apr 18 16:19:18 2017 +0000
+++ b/source/Movement.cpp	Tue Apr 18 17:33:22 2017 +0000
@@ -5,6 +5,13 @@
 
 #include "Movement.h"
 
+bool is_turning = false;
+bool direction = false;
+float wanted_deg = 0;
+float current_deg = 0;
+Timer t = 0;
+float previous_t = 0;
+
 int moving()
 {
 
@@ -23,10 +30,44 @@
     return 0;
 }
 
-int turn_for_deg(float deg)
+void turn_for_deg(float deg)
 {
 
-    return 0;
+    if(is_turning == false) {
+
+        is_turning = true;
+        float left = 0;
+        float right = 0;
+
+        wanted_deg = deg;
+        current_deg = 0;
+
+        if(deg < 0) { // turn left
+            direction = 1;
+            left = -50.0f;
+            right = 50.0f;
+        } else { // turn right
+            direction = 0;
+            left = 50.0f;
+            right = -50.0f;
+        }
+        set_speed(left, right);
+        t = 0;
+        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) {
+            set_speed(0,0);
+            is_turning = false;
+            t.stop();
+        }
+    }
 }
 
 
@@ -36,30 +77,38 @@
     float current_heading = get_current_heading();
     position current_pos = get_current_pos();
     position next_pos = get_next_pos();
-    
+
     float needed_heading = 0;
     float distance = 0;
-    
+
     // nord(-y) = 0 grad
-    if(current_pos.y > next_pos.y){
-        if(current_pos.x > next_pos.x) needed_heading = 315; distance = sqrt2;
-        if(current_pos.x == next_pos.x) needed_heading = 0; distance = 1;
-        if(current_pos.x < next_pos.x) needed_heading = 45; distance = sqrt2;
-        }
-    if(current_pos.y == next_pos.y){
-        if(current_pos.x > next_pos.x) needed_heading = 270; distance = 1;
+    if(current_pos.y > next_pos.y) {
+        if(current_pos.x > next_pos.x) needed_heading = 315;
+        distance = sqrt2;
+        if(current_pos.x == next_pos.x) needed_heading = 0;
+        distance = 1;
+        if(current_pos.x < next_pos.x) needed_heading = 45;
+        distance = sqrt2;
+    }
+    if(current_pos.y == next_pos.y) {
+        if(current_pos.x > next_pos.x) needed_heading = 270;
+        distance = 1;
         if(current_pos.x == next_pos.x) //error same position;
-        if(current_pos.x < next_pos.x) needed_heading = 90; distance = 1;
-        }
-    if(current_pos.y < next_pos.y){
-        if(current_pos.x > next_pos.x) needed_heading = 225; distance = sqrt2;
-        if(current_pos.x == next_pos.x) needed_heading = 180; distance = 1;
-        if(current_pos.x < next_pos.x) needed_heading = 135; distance = sqrt2;
-        }
+            if(current_pos.x < next_pos.x) needed_heading = 90;
+        distance = 1;
+    }
+    if(current_pos.y < next_pos.y) {
+        if(current_pos.x > next_pos.x) needed_heading = 225;
+        distance = sqrt2;
+        if(current_pos.x == next_pos.x) needed_heading = 180;
+        distance = 1;
+        if(current_pos.x < next_pos.x) needed_heading = 135;
+        distance = sqrt2;
+    }
 
     if(needed_heading != current_heading) {
         turn_for_deg((needed_heading-current_heading));
-    } else {  
+    } else {
         move_forward_for_distance(distance);
     }
     return 0;