with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: Sonar.cpp
- Revision:
- 33:814bcd7d3cfe
- Child:
- 34:c208497dd079
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sonar.cpp Fri Jun 09 00:28:32 2017 +0000 @@ -0,0 +1,104 @@ +#include "Sonar.hpp" + +Sonar::Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot ){ + this->anlgeFromCenter=anlgeFromCenter; + this->distanceXRobot=-distanceYRobot; + this->distanceYRobot=distanceXRobot; + this->maxRange=50;//cm + this->minRange=10;//Rmin cm + this->incertitudeRange=10;//cm + 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 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)); + 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; + 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; + + //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(anglePointToSonar <= this->angleRange/2 || anglePointToSonar >= this->rad_angle_check(-this->angleRange/2)){ + if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){ + //point before obstacle, probably empty + /*****************************************************************************/ + float Ea=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2); + float Er; + if(distancePointToSonar < this->minRange){ + //point before minimum sonar range + Er=0.f; + }else{ + //point after minimum sonar range + Er=1.f-pow((distancePointToSonar-this->minRange)/(distanceObstacleDetected-this->incertitudeRange-this->minRange),2); + } + /*****************************************************************************/ + //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); + return (1.f-Er*Ea)/2.f; + }else{ + //probably occupied + /*****************************************************************************/ + float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); + float Or; + if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){ + //point between distanceObstacleDetected +- INCERTITUDE_SONAR + Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(this->incertitudeRange),2); + }else{ + //point after in range of the sonar but after the zone detected + Or=0; + } + /*****************************************************************************/ + //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); + return (1+Or*Oa)/2; + } + } + } + //not checked by the sonar + return 0.5; +} + +//returns the angle between the vectors (x,y) and (xs,ys) +float Sonar::compute_angle_between_vectors(float x, float y,float xs,float ys){ + //alpha angle between ->x and ->SA + //vector S to A ->SA + float vSAx=x-xs; + float vSAy=y-ys; + //norme SA + float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2)); + //vector ->x (1,0) + float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;; + //vector ->y (0,1) + float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx; + if (sinAlpha < 0) + return -acos(cosAlpha); + else + return acos(cosAlpha); +} + +//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; + }else{ + while(inAngle < 0) + inAngle+=2*pi; + } + //cout<<" after :"<<inAngle<<endl; + return inAngle; +} +