Diff: source/Movement.cpp

Revision:

94:0381e8b1beda

Parent:

92:b2f4ed4cb1e5

Child:

96:1c6867536350

--- a/source/Movement.cpp	Tue May 02 13:25:56 2017 +0000
+++ b/source/Movement.cpp	Tue May 02 15:29:50 2017 +0000
@@ -17,6 +17,11 @@
 int search_state = 0;
 float restdeg = 0;
 
+float left = 0;
+float right = 0;
+
+bool devider = true;
+
 
 int moving()
 {
@@ -44,8 +49,6 @@
     if(distance != 0) {
 
         is_moving = true;
-        float left = 0;
-        float right = 0;
 
         wanted_dist = sqrt(distance*distance);
 
@@ -60,12 +63,25 @@
         }
         printf("set speed %f\r\n", left);
         set_speed(left, right);
+        devider = true;
         t.reset();
         t.start();
 
     } else {
+        float speed_multiplier = 0.6f;
+        if(wanted_dist < 0.10f && devider == true) {
+            //printf("devided\r\n");
+            devider = false;
+            if(direction) {  //move forward
+                left = left * speed_multiplier;
+                right = right * speed_multiplier;
+                //printf("left: %f || right: %f\r\n", left, right);
+            }
+            set_speed(left, right);
+        }
 
         float speed_left = get_speed_left();
+        printf("speed left: %f\r\n", speed_left);
         wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
         t.reset();
 
@@ -85,28 +101,35 @@
     if(deg != 0) {
 
         is_turning = true;
-        float left = 0;
-        float right = 0;
-
-        wanted_deg = sqrt(deg*deg);
+        wanted_deg = fabsf(deg);
 
         if(deg < 0) { // turn left
             direction = 1;
-            left = -50.0f;
-            right = 50.0f;
+            left = -30.0f;
+            right = 30.0f;
         } else { // turn right
             direction = 0;
-            left = 50.0f;
-            right = -50.0f;
+            left = 30.0f;
+            right = -30.0f;
         }
         set_speed(left, right);
+        devider = true;
         t.reset();
         t.start();
 
     } else {
-
+        float speed_multiplier = 0.6f;
+        if(wanted_deg < 10.0f && devider == true) {
+            devider = false;
+            if(direction) {  //move forward
+                left = left * speed_multiplier;
+                right = right * speed_multiplier;
+            }
+            set_speed(left, right);
+        }
+        
         float speed_left = get_speed_left();
-        wanted_deg -=  360.0f / (2*(float)wheel_r*(float)M_PI) * t.read() * fabsf(speed_left);
+        wanted_deg -= 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f);
         t.reset();
         if(wanted_deg <= 0) {
             set_speed(0,0);
@@ -114,6 +137,7 @@
             t.stop();
         }
     }
+    printf("remaining deg %f\r\n", wanted_deg);
     return (wanted_deg);
 }
 
@@ -222,8 +246,8 @@
             break;
 
         case 6: // Grabbing
-            return 52;                                                          //main state machine set as Grabbing
-            
+            return 50;                                                          //main state machine set as Grabbing
+
         default:
             // error
             break;