Diff: source/Movement.cpp

Revision:

74:d9c387b83196

Parent:

71:ddf4eb5c3081

Child:

76:bdbdd64cdd80

--- a/source/Movement.cpp	Fri Apr 21 12:37:07 2017 +0000
+++ b/source/Movement.cpp	Tue Apr 25 07:59:59 2017 +0000
@@ -4,6 +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
 
 bool is_turning = false;
 bool direction = false;
@@ -11,6 +12,9 @@
 float current_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 moving()
 {
@@ -114,9 +118,52 @@
     return 0;
 }
 
+// Tobias Berger
 int move_in_search_for_brick()
 {
+    
+    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
+        float restdeg=turn_for_deg(60);                                         // call function and start turning
+    }
+    
+    // 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
+    
+    if((lower<800.0)&&(lower>100)){                                             // if something is in the range of 10 to 80cm at the lower Sensor
+        if((upper>800.0)&&(upper<100)){                                         // and nothing is detected with the upper Sensor
+            brick_found = true;                                                   
+        }
+    }
+    else
+    {
+        brick_found = false;
+        
+        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
+        }                                                     
+                                                          
+    }
 
+    if(brick_found==true){
+        turn_for_deg(0);                                                        // stop turning
+        first_search_cyle=true;                                                 // set flag to start turning once again respectivly to get in Initialstate
+        lower=getDistaceIR(6);                                                  // Measure distance to Brick for Movement
+        distance_to_Brick = lower-OFFSET_GREIFER_TO_IRSENSOR;                   // calculate
+        move_forward_for_distance(distance_to_Brick);                           // 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;
+    }
+
+
+    
     return 0;
 }