Diff: source/Movement.cpp

Revision:

114:8e8a7a9b6fd8

Parent:

106:02d3327bf76a

--- a/source/Movement.cpp	Fri May 05 10:48:36 2017 +0000
+++ b/source/Movement.cpp	Fri May 05 12:36:55 2017 +0000
@@ -306,7 +306,7 @@
 {
     float upper = getDistanceIR(2);                                             // get distance from upper Center Sensor
     float lower = getDistanceIR(3);                                             // get distance from Lower Center Sensor
-    printf("Current Search State: >%d<\r\n",search_state);
+    //printf("Current Search State: >%d<\r\n",search_state);
     switch (search_state) {
         case 0: //first cycle right
             turn_for_deg(60.0f);                                               // call function and start turning
@@ -314,8 +314,8 @@
             break;
 
         case 1: // turn right 60 deg
-            if((lower<0.75f)&&(lower>0.05f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
-                if(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor
+            if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
+                if((lower+0.1f)<upper) {                                         // and nothing is detected with the upper Sensor
                     stop_turn();
                     search_state = 5;                                                 //brick found
                     printf("Brick found lower: %f upper:%f",lower,upper);
@@ -336,7 +336,7 @@
 
         case 3: // turn left 120 deg
             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
+                if((lower+0.1f)<upper) {                                         // and nothing is detected with the upper Sensor
                     stop_turn();
                     search_state = 10;                                                  //brick found
                     printf("Brick found lower: %f upper:%f",lower,upper);
@@ -359,7 +359,7 @@
             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
                     stop_turn();
-                    search_state = 10;                                                  //brick found
+                    search_state = 10;                                          //brick found
                 }
             } else {
                 search_state = 8;                                               // go to same state
@@ -373,6 +373,7 @@
 
         case 10: // first cycle move
             float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
+            printf("Distance to Brick: %f",distance_to_Brick);                  
             move_for_distance(distance_to_Brick);
             search_state =11;
             break;