with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: Sonar.cpp
- Revision:
- 34:c208497dd079
- Parent:
- 33:814bcd7d3cfe
- Child:
- 35:68f9edbb3cff
diff -r 814bcd7d3cfe -r c208497dd079 Sonar.cpp --- 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;