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
             /*****************************************************************************/