Diff: source/Movement.cpp

Revision:

79:50f8d58f79ab

Parent:

77:ff87a10c4baf

Child:

83:5d3bca1ece20

diff -r d9138c6f82a8 -r 50f8d58f79ab source/Movement.cpp
--- a/source/Movement.cpp	Tue Apr 25 11:42:45 2017 +0000
+++ b/source/Movement.cpp	Mon May 01 09:25:06 2017 +0000
@@ -125,15 +125,15 @@
     // 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
+        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
+    float upper = getDistanceIR(2);                                             // get distance from upper Center Sensor
+    float lower = getDistanceIR(3);                                             // 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
+    if((lower<0.8f)&&(lower>0.1f)){                                             // if something is in the range of 10 to 80cm at the lower Sensor
+        if((upper-lower)>0.01)){                                                // and nothing is detected with the upper Sensor
             brick_found = true;                                                   
         }
     }
@@ -141,17 +141,19 @@
     {
         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
+            printf("Continue turning\n\r");                                     
             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
+            restdeg=-120;                                                       //                                                                                      60 DEGREES FROM YET WILL BE THE SAME AREA AS PREVIOUSLY
         }                                                     
                                                           
     }
 
     if(brick_found==true){
+        printf("Brick found entered\n\r");
         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