Diff: source/Movement.cpp

Revision:

108:d18a2beb9b9b

Parent:

107:02bc5b4e67b7

Child:

109:510927a51be8

diff -r 02bc5b4e67b7 -r d18a2beb9b9b source/Movement.cpp
--- a/source/Movement.cpp	Fri May 05 13:31:04 2017 +0000
+++ b/source/Movement.cpp	Sat May 06 21:07:43 2017 +0000
@@ -5,7 +5,7 @@
 
 #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_WHEELS 0.09                              // Offset of the wheels from the center pos
+#define OFFSET_WHEELS 0.09                              // Offset of the wheels from the max pos
 
 bool is_moving = false;
 float wanted_dist = 0;
@@ -68,14 +68,19 @@
         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;
-        if(fabsf(r) < 0.2f) center_speed *= 0.5f;
-        float inner_speed = center_speed/circumference*circumference_inner;
-        float outer_speed = center_speed/circumference*circumference_outer;
+        float max_speed = 50;
+        float inner_speed = max_speed/circumference*circumference_inner;
+        float outer_speed = max_speed/circumference*circumference_outer;
 
-        //wanted_deg = 360 / (2*radius*(float)M_PI) * wanted_dist;
-        //float time = (10*wanted_dist)/(wheel_r * center_speed);
+        //reduce outer speed to max speed
+        float multiplier = 1.0f/inner_speed*max_speed;
+        inner_speed *= multiplier;
+        outer_speed *= multiplier;
 
+        if(r < 0.21f) {
+            outer_speed *= 0.5f;
+            inner_speed *= 0.5f;
+        }
 
         if(r != 0) {
             //move with turn
@@ -93,12 +98,12 @@
             printf("move straight\r\n");
             if(distance > 0) {  //move forward
                 direction = 1;
-                left = center_speed;
-                right = center_speed;
+                left = max_speed;
+                right = max_speed;
             } else {            //move backward
                 direction = 0;
-                left = -center_speed;
-                right = -center_speed;
+                left = -max_speed;
+                right = -max_speed;
             }
         }
 
@@ -292,8 +297,8 @@
 **/
 int move_in_search_for_brick()
 {
-    float upper = getDistanceIR(2);                                             // get distance from upper Center Sensor
-    float lower = getDistanceIR(3);                                             // get distance from Lower Center Sensor
+    float upper = getDistanceIR(2);                                             // get distance from upper max Sensor
+    float lower = getDistanceIR(3);                                             // get distance from Lower max Sensor
     printf("Current Search State: >%d<\r\n",search_state);
     switch (search_state) {
         case 0: //first cycle right