Diff: main.cpp

Revision:

48:cd3463dcca04

Parent:

47:f0fe58571e4a

Child:

49:d61da2bc8882

--- a/main.cpp	Tue May 30 15:17:19 2017 +0000
+++ b/main.cpp	Tue May 30 15:51:38 2017 +0000
@@ -354,7 +354,14 @@
     float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
     anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
     float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
-
+    
+    if(alphaBeforeAdjustment>pi)
+        alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
+    if(alphaBeforeAdjustment<-pi)
+        alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
+        
+    //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
+    
     //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
     //check if absolute difference between the angles is no more than Omega/2
     if( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2))){
@@ -707,7 +714,7 @@
     float angularLeft=0;
 
     go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
-    pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
+    //pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
 
     leftMotor(sign2(angularLeft),abs(angularLeft));
     rightMotor(sign2(angularRight),abs(angularRight));
@@ -779,7 +786,7 @@
     bool inFront=true;
     //atan2 gives the angle beetween pi and -pi
     float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
-    pc.printf("angleToTarget=%f",angleToTarget);
+    //pc.printf("angleToTarget=%f",angleToTarget);
     if(angleToTarget < 0)//the target is in front
         inFront=false;