Diff: source/Movement.cpp

Revision:

99:78d87027c85b

Parent:

96:1c6867536350

Child:

101:6b10685aa34d

--- a/source/Movement.cpp	Wed May 03 06:59:34 2017 +0000
+++ b/source/Movement.cpp	Wed May 03 08:47:21 2017 +0000
@@ -72,11 +72,9 @@
         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);
-            }
+            left = left * speed_multiplier;
+            right = right * speed_multiplier;
+            //printf("left: %f || right: %f\r\n", left, right);
             set_speed(left, right);
         }
 
@@ -121,10 +119,8 @@
         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;
-            }
+            left = left * speed_multiplier;
+            right = right * speed_multiplier;
             set_speed(left, right);
         }
         
@@ -202,6 +198,7 @@
                 if(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor
                     stop_turn();
                     search_state = 4;                                                 //brick found
+                    printf("Brick found lower: %f upper:%f",lower,upper);
                 }
             } else {
                 search_state = 1;                                               // go to same state
@@ -222,6 +219,7 @@
                 if(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor
                     stop_turn();
                     search_state = 4;                                                  //brick found
+                    printf("Brick found lower: %f upper:%f",lower,upper);       
                 }
             } else {
                 search_state = 3;                                               // go to same state