Diff: source/Movement.cpp

Revision:

85:d8ea8a99fa3a

Parent:

81:956f65714207

Child:

86:df8c869a5a52

--- a/source/Movement.cpp	Mon May 01 07:39:18 2017 +0000
+++ b/source/Movement.cpp	Mon May 01 10:07:35 2017 +0000
@@ -6,12 +6,15 @@
 #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_moving = false;
+float wanted_dist = 0;
+
 bool is_turning = false;
 bool direction = false;
 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 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
 float restdeg = 0;
@@ -23,10 +26,42 @@
     return 0;
 }
 
-int move_forward_for_distance(float distance)
+float move_for_distance(float distance)
 {
+    if(distance != 0) {
 
-    return 0;
+        is_moving = true;
+        float left = 0;
+        float right = 0;
+
+        wanted_dist = sqrt(distance*distance);
+
+        if(distance > 0) {  //move forward
+            direction = 1;
+            left = 50.0f;
+            right = 50.0f;
+        } else {            //move backward
+            direction = 0;
+            left = -50.0f;
+            right = -50.0f;
+        }
+        set_speed(left, right);
+        t.reset();
+        t.start();
+
+    } else {
+
+        float speed_left = get_speed_left();
+        wanted_dist -= (2*(float)wheel_r*(float)M_PI) * t.read() * fabsf(speed_left);
+        t.reset();
+
+        if(wanted_dist <= 0) { //distance covered, Stop function
+            set_speed(0,0);
+            is_moving = false;
+            t.stop();
+        }
+    }
+    return wanted_dist;
 }
 
 int move_backward_for_distance()
@@ -35,7 +70,7 @@
     return 0;
 }
 
-float turn_for_deg(float deg) //if deg not 0 equals initilisation.
+float turn_for_deg(float deg)   //if deg not 0 equals initilisation.
 {
 
     if(deg != 0) {
@@ -62,7 +97,7 @@
     } else {
 
         float speed_left = get_speed_left();
-        wanted_deg -=  360.0f / (2*radius*M_PI) * t.read() * fabsf(speed_left);
+        wanted_deg -=  360.0f / (2*(float)wheel_r*(float)M_PI) * t.read() * fabsf(speed_left);
         t.reset();
         if(wanted_deg <= 0) {
             set_speed(0,0);
@@ -112,7 +147,7 @@
     if(needed_heading != current_heading) {
         turn_for_deg((needed_heading-current_heading));
     } else {
-        move_forward_for_distance(distance);
+        move_for_distance(distance);
     }
     return 0;
 }
@@ -120,49 +155,45 @@
 // 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){
+    if(first_search_cycle==true) {
         first_search_cycle=false;                                               // delet flag for initial condition
         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.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;                                                   
+
+    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
-    {
+    } else {
         brick_found = false;
-        
-        if((restdeg>1)||(restdeg<-1)){                                             // continue turning until restdegree nearly 0                                     
+
+        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
         }
-        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){
+    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_forward_for_distance(distance_to_Brick); //not whole distance, rest in next function // Move to Brick                                     ATTENTION FUNCTION NOT IMPLEMENTED YET
+        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;
     }
 
 
-    
+
     return 0;
 }