Diff: source/Movement.cpp

Revision:

108:02bc5b4e67b7

Parent:

106:02d3327bf76a

Child:

109:d18a2beb9b9b

--- a/source/Movement.cpp	Fri May 05 10:48:36 2017 +0000
+++ b/source/Movement.cpp	Fri May 05 13:31:04 2017 +0000
@@ -69,6 +69,7 @@
         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;
 
@@ -76,10 +77,9 @@
         //float time = (10*wanted_dist)/(wheel_r * center_speed);
 
 
-        if(r > 0) {
-
-        //turn right
-        if(distance > 0) {  //move forward
+        if(r != 0) {
+            //move with turn
+            if(distance > 0) {  //move forward
                 direction = 1;
                 left = outer_speed;
                 right = inner_speed;
@@ -88,21 +88,9 @@
                 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
-
+            printf("move straight\r\n");
             if(distance > 0) {  //move forward
                 direction = 1;
                 left = center_speed;
@@ -206,7 +194,7 @@
  * if deg not 0: turn initilisation.
  * Claudio Citterio
 **/
-float turn_for_deg(float deg)   
+float turn_for_deg(float deg)
 {
 
     if(deg != 0) {
@@ -298,7 +286,7 @@
     return 0;
 }
 
-/** 
+/**
  * this function searchs a nearby brick, moves towards it and grabbs it
  * by Tobias Berger, state machine by Claudio Citterio
 **/