Diff: source/Movement.cpp

Revision:

86:df8c869a5a52

Parent:

85:d8ea8a99fa3a

Parent:

83:5d3bca1ece20

Child:

88:b89cace9329b

--- a/source/Movement.cpp	Mon May 01 10:07:35 2017 +0000
+++ b/source/Movement.cpp	Mon May 01 12:55:11 2017 +0000
@@ -4,7 +4,7 @@
 **/
 
 #include "Movement.h"
-#define OFFSET_GREIFER_TO_IRSENSOR 100                                          // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
+#define OFFSET_GREIFER_TO_IRSENSOR 0.15                                          // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
 
 bool is_moving = false;
 float wanted_dist = 0;
@@ -14,9 +14,7 @@
 float wanted_deg = 0;
 Timer t;
 float previous_t = 0;
-bool first_search_cycle = true;                                                 // flag for state first time in function "move in search for brick"
-bool brick_found = false;                                                       // flag for saving whether a brick was found or not
-bool movement_to_brick_finished = false;                                        // flag for saving whether movement to brick is finished or not
+int search_state = 0;
 float restdeg = 0;
 
 
@@ -26,6 +24,20 @@
     return 0;
 }
 
+void stop_move()
+{
+    set_speed(0,0);
+    wanted_dist = 0;
+    is_moving = false;
+}
+
+void stop_turn()
+{
+    set_speed(0,0);
+    wanted_deg = 0;
+    is_turning = false;
+}
+
 float move_for_distance(float distance)
 {
     if(distance != 0) {
@@ -155,46 +167,71 @@
 // Tobias Berger
 int move_in_search_for_brick()
 {
+    float upper = getDistanceIR(2);                                             // get distance from upper Center Sensor
+    float lower = getDistanceIR(3);                                             // get distance from Lower Center Sensor
 
-    float distance_to_Brick;                                                    // variable how far away the brick is
-    // Init State turn for 60 degrees CW
-    if(first_search_cycle==true) {
-        first_search_cycle=false;                                               // delet flag for initial condition
-        restdeg=turn_for_deg(60);                                         // call function and start turning
-    }
+    switch (search_state) {
+        case 0: //first cycle right
+            turn_for_deg(60.0f);                                               // call function and start turning
+            search_state = 1;
+            break;
 
-    // Search for Brick and evaluation
-    float upper = getDistanceIR(4);                                             // get distance from upper Center Sensor            CHECK SENSORNUMBERS NOT SURE
-    float lower = getDistanceIR(6);                                             // get distance from Lower Center Sensor
+        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(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor
+                    stop_turn();
+                    search_state = 4;                                                 //brick found
+                }
+            } else {
+                search_state = 1;                                               // go to same state
+                if(turn_for_deg(0) < 0) {
+                    stop_turn();
+                    search_state = 2;
+                }
+            }
+            break;
 
-    if((lower<800.0f)&&(lower>100.0f)) {                                            // if something is in the range of 10 to 80cm at the lower Sensor
-        if((upper>800.0f)&&(upper<100.0f)) {                                        // and nothing is detected with the upper Sensor
-            brick_found = true;
-        }
-    } else {
-        brick_found = false;
+        case 2: // first cycle left
+            turn_for_deg(-120.0f);
+            search_state = 3;
+            break;
 
-        if((restdeg>1)||(restdeg<-1)) {                                            // continue turning until restdegree nearly 0
-            turn_for_deg(restdeg);
-        } else {                                                                // if restdegree nearly 0 and nothing found => turn in other direction
-            restdeg=-60;                                                        //                                                                                      60 DEGREES FROM YET WILL BE THE SAME AREA AS PREVIOUSLY
-        }
-
-    }
+        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(fabsf((upper-lower))>0.02f) {                                         // and nothing is detected with the upper Sensor
+                    stop_turn();
+                    search_state = 4;                                                  //brick found
+                }
+            } else {
+                search_state = 3;                                               // go to same state
+                if(turn_for_deg(0) < 0) {
+                    stop_turn();
+                    search_state = 10;                                          // error
+                }
+            }
+            break;
 
-    if(brick_found==true) {
-        turn_for_deg(0);                                                        // stop turning
-        first_search_cycle=true;                                                 // set flag to start turning once again respectivly to get in Initialstate
-        lower=getDistanceIR(6);                                                  // Measure distance to Brick for Movement
-        distance_to_Brick = lower-OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
-        move_for_distance(distance_to_Brick);                                   //not whole distance, rest in next function // Move to Brick                                     ATTENTION FUNCTION NOT IMPLEMENTED YET
-        arm_position_grabbing();                                                // Call Aeschlimans function                         MOVE A LITTLE AFTER GREIFER ON FLOOR IN AESCHLIMANS FUNCTION?
-        //movement_to_brick_finished=true;
+        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;
+            break;
+
+        case 5: // move forward
+            if(move_for_distance(0) < 0) {
+                stop_move();
+                search_state = 6;
+            }
+            break;
+
+        case 6: // Grabbing
+            return 52;                                                          //main state machine set as Grabbing
+            
+        default:
+            // error
+            break;
     }
-
-
-
-    return 0;
+    return 47;                                                                  //called until function is done
 }