Diff: source/Movement.cpp

Revision:

105:d489c2e8de35

Parent:

104:7bc5eb2b4199

Child:

106:02d3327bf76a

--- a/source/Movement.cpp	Wed May 03 09:56:49 2017 +0000
+++ b/source/Movement.cpp	Thu May 04 09:19:26 2017 +0000
@@ -4,18 +4,18 @@
 **/
 
 #include "Movement.h"
-#define OFFSET_GREIFER_TO_IRSENSOR 0.15                                          // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
+#define OFFSET_GREIFER_TO_IRSENSOR 0.15                 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
+#define OFFSET_WHEELS 0.09                              // Offset of the wheels from the center pos
 
 bool is_moving = false;
 float wanted_dist = 0;
-
 bool is_turning = false;
-bool direction = false;
 float wanted_deg = 0;
+bool direction = false;
+
 Timer t;
-float previous_t = 0;
+
 int search_state = 0;
-float restdeg = 0;
 
 float left = 0;
 float right = 0;
@@ -43,6 +43,95 @@
     is_turning = false;
 }
 
+float move_for_distance_with_radius(float distance, float r)
+{
+
+    if(distance != 0) {
+
+        is_moving = true;
+        wanted_dist = fabsf(distance);
+
+        float circumference = r*2*(float)M_PI;
+        float circumference_inner = ((r-(float)OFFSET_WHEELS)*2*(float)M_PI);
+        float circumference_outer = ((r+(float)OFFSET_WHEELS)*2*(float)M_PI);
+
+        float center_speed = 50;
+        float inner_speed = center_speed/circumference*circumference_inner;
+        float outer_speed = center_speed/circumference*circumference_outer;
+
+        //wanted_deg = 360 / (2*radius*(float)M_PI) * wanted_dist;
+        //float time = (10*wanted_dist)/(wheel_r * center_speed);
+
+
+        if(r > 0) {
+
+        //turn right
+        if(distance > 0) {  //move forward
+                direction = 1;
+                left = outer_speed;
+                right = inner_speed;
+            } else {            //move backward
+                direction = 0;
+                left = -outer_speed;
+                right = -inner_speed;
+            }
+        } else if(r < 0) {
+
+        // turn left
+        if(distance > 0) {  //move forward
+                direction = 1;
+                left = inner_speed;
+                right = outer_speed;
+            } else {            //move backward
+                direction = 0;
+                left = -inner_speed;
+                right = -outer_speed;
+            }
+        } else {
+            //normal straight movement
+
+            if(distance > 0) {  //move forward
+                direction = 1;
+                left = center_speed;
+                right = center_speed;
+            } else {            //move backward
+                direction = 0;
+                left = -center_speed;
+                right = -center_speed;
+            }
+        }
+
+        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;
+            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();
+        float speed_right = get_speed_right();
+        wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * ((fabsf(speed_left)+fabsf(speed_right))/2) * 0.1f;
+        t.reset();
+
+        if(wanted_dist <= 0) { //distance covered, Stop function
+            set_speed(0,0);
+            is_moving = false;
+            t.stop();
+        }
+    }
+    printf("remaining distance to cover: %f\r\n", wanted_dist);
+    return wanted_dist;
+}
+
+
 float move_for_distance(float distance)
 {
     printf("move for distance\r\n");
@@ -50,7 +139,7 @@
 
         is_moving = true;
 
-        wanted_dist = sqrt(distance*distance);
+        wanted_dist = fabsf(distance);
 
         if(distance > 0) {  //move forward
             direction = 1;
@@ -234,7 +323,7 @@
             turn_for_deg(-10.0f);
             search_state = 8;
             break;
-            
+
         case 8: // turn back right continue
             if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 75cm at the lower Sensor
                 if(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor