Diff: Sonar.cpp

Revision:

34:c208497dd079

Parent:

33:814bcd7d3cfe

Child:

35:68f9edbb3cff

--- a/Sonar.cpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/Sonar.cpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,9 +1,11 @@
 #include "Sonar.hpp"
 
-Sonar::Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot ){
-	this->anlgeFromCenter=anlgeFromCenter;
-	this->distanceXRobot=-distanceYRobot;
-	this->distanceYRobot=distanceXRobot;
+#define PI 3.14159
+
+Sonar::Sonar(float angleFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){
+	this->angleFromCenter=angleFromCenter;
+	this->distanceX=-distanceYFromRobotCenter;
+	this->distanceY=distanceXFromRobotCenter;
 	this->maxRange=50;//cm
 	this->minRange=10;//Rmin cm
 	this->incertitudeRange=10;//cm
@@ -11,20 +13,20 @@
 }
 
 //ODOMETRIA MUST HAVE BEEN CALLED
-//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
-float Sonar::compute_probability_t(float distanceObstacleDetected, float x, float y, float robotCoordinateXWorld, float robotCoordinateYWorld){
-	float xSonar=robotCoordinateXWorld+this->distanceX;
-	float ySonar=robotCoordinateYWorld+this->distanceY;
-    float distancePointToSonar=sqrt(pow(x-xSonar,2)+pow(y-ySonar,2));
+//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left PI/3, right -PI/3) returns the probability it's occuPIed/empty [0;1]
+float Sonar::compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float theta){
+	float xSonar=xRobotWorld+this->distanceX;
+	float ySonar=yRobotWorld+this->distanceY;
+    float distancePointToSonar=sqrt(pow(xCell-xSonar,2)+pow(yCell-ySonar,2));
     if( distancePointToSonar < this->maxRange){
-        float anglePointToSonar=this->compute_angle_between_vectors(x,y,xSonar,ySonar);//angle beetween the point and the sonar beam
-        float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
+        float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam
+        float alphaBeforeAdjustment=anglePointToSonar-theta-this->angleFromCenter;
         anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
         
-        if(alphaBeforeAdjustment>pi)
-            alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
-        if(alphaBeforeAdjustment<-pi)
-            alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
+        if(alphaBeforeAdjustment>PI)
+            alphaBeforeAdjustment=alphaBeforeAdjustment-2*PI;
+        if(alphaBeforeAdjustment<-PI)
+            alphaBeforeAdjustment=alphaBeforeAdjustment+2*PI;
             
         //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
         
@@ -48,9 +50,9 @@
                 //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
                 return (1.f-Er*Ea)/2.f;
             }else{
-                //probably occupied
+                //probably occuPIed
             /*****************************************************************************/
-                float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
+                float Oa=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
                 float Or;
                 if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){
                     //point between distanceObstacleDetected +- INCERTITUDE_SONAR
@@ -88,15 +90,15 @@
         return acos(cosAlpha);
 }
 
-//makes the angle inAngle between 0 and 2pi
+//makes the angle inAngle between 0 and 2PI
 float Sonar::rad_angle_check(float inAngle){
     //cout<<"before :"<<inAngle;
     if(inAngle > 0){
-        while(inAngle > (2*pi))
-            inAngle-=2*pi;
+        while(inAngle > (2*PI))
+            inAngle-=2*PI;
     }else{
         while(inAngle < 0)
-            inAngle+=2*pi;
+            inAngle+=2*PI;
     }
     //cout<<" after :"<<inAngle<<endl;
     return inAngle;