Diff: Sonar.cpp

Revision:

37:b4c45e43ad29

Parent:

35:68f9edbb3cff

Child:

38:5ed7c79fb724

--- a/Sonar.cpp	Sun Jun 11 22:53:59 2017 +0000
+++ b/Sonar.cpp	Sun Jun 11 23:22:28 2017 +0000
@@ -2,8 +2,8 @@
 
 #define PI 3.14159
 
-Sonar::Sonar(float angleFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){
-	this->angleFromCenter=angleFromCenter;
+Sonar::Sonar(float angleFromOrigin, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){
+	this->angleFromOrigin=angleFromOrigin;
 	this->distanceX=distanceXFromRobotCenter;
 	this->distanceY=distanceYFromRobotCenter;
 	this->maxRange=50;//cm
@@ -12,31 +12,25 @@
 	this->angleRange=3.14159/3;//Omega rad
 }
 
-//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 xCell, float yCell, float xRobotWorld, float yRobotWorld, float theta){
+float Sonar::compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float thetaWorld){
 	float xSonar=xRobotWorld+this->distanceX;
 	float ySonar=yRobotWorld+this->distanceY;
     float distancePointToSonar=sqrt(pow(xCell-xSonar,2)+pow(yCell-ySonar,2));
+    //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
     if( distancePointToSonar < this->maxRange){
-        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
+        //float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam
+        float anglePointToSonar=atan2(yCell-yRobotWorld,xCell-xRobotWorld);//like world system
         
-        if(alphaBeforeAdjustment>PI)
-            alphaBeforeAdjustment=alphaBeforeAdjustment-2*PI;
-        if(alphaBeforeAdjustment<-PI)
-            alphaBeforeAdjustment=alphaBeforeAdjustment+2*PI;
+       	float angleOriginToMidleOfBeam=thetaWorld+this->angleFromOrigin;// 
             
-        //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
+        float angleDifference=anglePointToSonar-angleOriginToMidleOfBeam;
         //check if absolute difference between the angles is no more than Omega/2
-        if(anglePointToSonar <= this->angleRange/2 || anglePointToSonar >= this->rad_angle_check(-this->angleRange/2)){
+        if(abs(angleDifference) <= this->angleRange/2){
             if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){
             //point before obstacle, probably empty
             /*****************************************************************************/
-                float Ea=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
+                float Ea=1.f-pow((2*angleDifference)/this->angleRange,2);
                 float Er;
                 if(distancePointToSonar < this->minRange){
                     //point before minimum sonar range
@@ -47,12 +41,12 @@
                 }
              /*****************************************************************************/
                 //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
-                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
+                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,angleDifference=%f",(1.f-Er*Ea)/2.f,Er,Ea,angleDifference);
                 return (1.f-Er*Ea)/2.f;
             }else{
                 //probably occuPIed
             /*****************************************************************************/
-                float Oa=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
+                float Oa=1.f-pow((2*angleDifference)/this->angleRange,2);
                 float Or;
                 if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){
                     //point between distanceObstacleDetected +- INCERTITUDE_SONAR
@@ -63,7 +57,7 @@
                 }
             /*****************************************************************************/
                 //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
-                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
+                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,angleDifference=%f",(1+Or*Oa)/2,Or,Oa,angleDifference);
                 return (1+Or*Oa)/2;
             }
         }