Diff: source/Movement.cpp

Revision:

112:c7afe49752b9

Parent:

110:cd7b7b551616

Child:

115:720dc5df42a5

--- a/source/Movement.cpp	Mon May 08 12:09:39 2017 +0000
+++ b/source/Movement.cpp	Tue May 09 13:58:57 2017 +0000
@@ -14,6 +14,7 @@
 bool direction = false;
 
 Timer t;
+Timer t8;
 
 int search_state = 0;
 
@@ -78,8 +79,8 @@
         outer_speed *= multiplier;
 
         if(r < 0.21f) {
-            outer_speed *= 0.5f;
-            inner_speed *= 0.5f;
+            outer_speed *= 0.8f;
+            inner_speed *= 0.8f;
         }
 
         if(r != 0) {
@@ -209,12 +210,12 @@
 
         if(deg < 0) { // turn left
             direction = 1;
-            left = -30.0f;
-            right = 30.0f;
+            left = -20.0f;
+            right = 20.0f;
         } else { // turn right
             direction = 0;
-            left = 30.0f;
-            right = -30.0f;
+            left = 20.0f;
+            right = -20.0f;
         }
         set_speed(left, right);
         devider = true;
@@ -299,7 +300,7 @@
 {
     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);
+    //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
@@ -307,8 +308,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.45f)&&(lower>0.05f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
+                if(fabsf((upper-lower))>0.05f) {                                         // and nothing is detected with the upper Sensor
                     stop_turn();
                     search_state = 5;                                                 //brick found
                     printf("Brick found lower: %f upper:%f",lower,upper);
@@ -328,10 +329,12 @@
             break;
 
         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((lower<0.45f)&&(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
+                    t8.reset();
+                    t8.start();
+                    search_state = 9;                                                  //brick found
                     printf("Brick found lower: %f upper:%f",lower,upper);
                 }
             } else {
@@ -348,21 +351,33 @@
             search_state = 8;
             break;
 
-        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
+        case 8: // turn back left continue
+            if((lower<0.45f)&&(lower>0.1f)) {                                            // if something is in the range of 10 to 75cm at the lower Sensor
+                if(fabsf((upper-lower))>0.05f) {                                         // and nothing is detected with the upper Sensor
                     stop_turn();
-                    search_state = 10;                                                  //brick found
+                    t8.reset();
+                    t8.start();
+                    search_state = 9;                                                  //brick found
                 }
             } else {
                 search_state = 8;                                               // go to same state
                 if(turn_for_deg(0) < 0) {
                     stop_turn();
-                    search_state = 20;                                          // error
+                    search_state = 0;                                          // error
                 }
             }
             break;
 
+        case 9:
+            // printf("timer: %f\r\n", t8.read());
+            if(t8.read() > 2.0f) {
+                search_state = 10;
+                t8.stop();
+                if(lower > 0.45f){
+                    search_state = 0;
+                }
+            }
+            break;
 
         case 10: // first cycle move
             float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
@@ -373,7 +388,7 @@
         case 11: // move forward
             if(move_for_distance(0) < 0) {
                 //Safety Function:
-                if (getDistanceIR(2)<0.05f) {
+                if (getDistanceIR(2)<0.08f) {
                     stop_move();
                     //move_for_distance(-0.10f);
                     search_state = 0;