Diff: source/Movement.cpp

Revision:

101:6b10685aa34d

Parent:

99:78d87027c85b

Child:

104:7bc5eb2b4199

--- a/source/Movement.cpp	Wed May 03 08:59:48 2017 +0000
+++ b/source/Movement.cpp	Wed May 03 09:24:52 2017 +0000
@@ -123,7 +123,7 @@
             right = right * speed_multiplier;
             set_speed(left, right);
         }
-        
+
         float speed_left = get_speed_left();
         wanted_deg -= 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f);
         t.reset();
@@ -194,10 +194,10 @@
             break;
 
         case 1: // turn right 60 deg
-            if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
+            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
                     stop_turn();
-                    search_state = 4;                                                 //brick found
+                    search_state = 5;                                                 //brick found
                     printf("Brick found lower: %f upper:%f",lower,upper);
                 }
             } else {
@@ -215,35 +215,60 @@
             break;
 
         case 3: // turn left 120 deg
-            if((lower<0.75f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
+            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 = 4;                                                  //brick found
-                    printf("Brick found lower: %f upper:%f",lower,upper);       
+                    search_state = 6;                                                  //brick found
+                    printf("Brick found lower: %f upper:%f",lower,upper);
                 }
             } else {
                 search_state = 3;                                               // go to same state
                 if(turn_for_deg(0) < 0) {
                     stop_turn();
-                    search_state = 10;                                          // error
+                    search_state = 20;                                          // error
                 }
             }
             break;
 
-        case 4: // first cycle move
-            float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
-            move_for_distance(distance_to_Brick);
-            search_state = 5;
+        case 5: // turn back left
+            turn_for_deg(-10.0f);
+            search_state = 8;
+            break;
+        case 6: // turn back right
+            turn_for_deg(10.0f);
+            search_state = 8;
             break;
-
-        case 5: // move forward
-            if(move_for_distance(0) < 0) {
-                stop_move();
-                search_state = 6;
+            
+        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
+                    stop_turn();
+                    search_state = 10;                                                  //brick found
+                }
+            } else {
+                search_state = 6;                                               // go to same state
+                if(turn_for_deg(0) < 0) {
+                    stop_turn();
+                    search_state = 20;                                          // error
+                }
             }
             break;
 
-        case 6: // Grabbing
+
+        case 10: // first cycle move
+            float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
+            move_for_distance(distance_to_Brick);
+            search_state =11;
+            break;
+
+        case 11: // move forward
+            if(move_for_distance(0) < 0) {
+                stop_move();
+                search_state = 12;
+            }
+            break;
+
+        case 12: // Grabbing
             return 50;                                                          //main state machine set as Grabbing
 
         default: