with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: Sonar.cpp
- Revision:
- 38:5ed7c79fb724
- Parent:
- 37:b4c45e43ad29
--- a/Sonar.cpp Sun Jun 11 23:22:28 2017 +0000 +++ b/Sonar.cpp Thu Jun 15 23:17:55 2017 +0000 @@ -2,8 +2,8 @@ #define PI 3.14159 -Sonar::Sonar(float angleFromOrigin, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){ - this->angleFromOrigin=angleFromOrigin; +Sonar::Sonar(float angleFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){ + this->angleFromCenter=angleFromCenter; this->distanceX=distanceXFromRobotCenter; this->distanceY=distanceYFromRobotCenter; this->maxRange=50;//cm @@ -12,6 +12,32 @@ this->angleRange=3.14159/3;//Omega rad } +//return distance sonar to cell if in range, -1 if not +float Sonar::isInRange(float xCell, float yCell, float xRobotWorld, float yRobotWorld, float thetaWorld){ + float xSonar=xRobotWorld+this->distanceX; + float ySonar=yRobotWorld+this->distanceY; + float distanceCellToSonar=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( distanceCellToSonar < this->maxRange){ + //float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam + float angleCellToSonar=atan2(yCell-yRobotWorld,xCell-xRobotWorld);//like world system + + float angleOriginToMidleOfBeam=thetaWorld+this->angleFromCenter;// + + float angleDifference=angleCellToSonar-angleOriginToMidleOfBeam; + if(angleDifference > PI) + angleDifference=angleDifference-2*PI; + if(angleDifference < -PI) + angleDifference=angleDifference+2*PI; + //check if absolute difference between the angles is no more than Omega/2 + if(angleDifference > 0 && angleDifference <= this->angleRange/2 ||angleDifference < 0 && angleDifference >= -this->angleRange/2 ){ + return distanceCellToSonar; + } + } + return -1; +} + //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 thetaWorld){ float xSonar=xRobotWorld+this->distanceX; @@ -22,11 +48,16 @@ //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 - float angleOriginToMidleOfBeam=thetaWorld+this->angleFromOrigin;// - + float angleOriginToMidleOfBeam=thetaWorld+this->angleFromCenter;// + float angleDifference=anglePointToSonar-angleOriginToMidleOfBeam; + if(angleDifference > PI) + angleDifference=angleDifference-2*PI; + if(angleDifference < -PI) + angleDifference=angleDifference+2*PI; //check if absolute difference between the angles is no more than Omega/2 - if(abs(angleDifference) <= this->angleRange/2){ + if(angleDifference > 0 && angleDifference <= this->angleRange/2 ||angleDifference < 0 && angleDifference >= -this->angleRange/2 ){ + if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){ //point before obstacle, probably empty /*****************************************************************************/