Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Sonar.cpp@33:814bcd7d3cfe, 2017-06-09 (annotated)

Committer:
Ludwigfr
Date:
Fri Jun 09 00:28:32 2017 +0000
Revision:
33:814bcd7d3cfe
Child:
34:c208497dd079
version with class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 33:814bcd7d3cfe 1 #include "Sonar.hpp"
Ludwigfr 33:814bcd7d3cfe 2
Ludwigfr 33:814bcd7d3cfe 3 Sonar::Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot ){
Ludwigfr 33:814bcd7d3cfe 4 this->anlgeFromCenter=anlgeFromCenter;
Ludwigfr 33:814bcd7d3cfe 5 this->distanceXRobot=-distanceYRobot;
Ludwigfr 33:814bcd7d3cfe 6 this->distanceYRobot=distanceXRobot;
Ludwigfr 33:814bcd7d3cfe 7 this->maxRange=50;//cm
Ludwigfr 33:814bcd7d3cfe 8 this->minRange=10;//Rmin cm
Ludwigfr 33:814bcd7d3cfe 9 this->incertitudeRange=10;//cm
Ludwigfr 33:814bcd7d3cfe 10 this->angleRange=3.14159/3;//Omega rad
Ludwigfr 33:814bcd7d3cfe 11 }
Ludwigfr 33:814bcd7d3cfe 12
Ludwigfr 33:814bcd7d3cfe 13 //ODOMETRIA MUST HAVE BEEN CALLED
Ludwigfr 33:814bcd7d3cfe 14 //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]
Ludwigfr 33:814bcd7d3cfe 15 float Sonar::compute_probability_t(float distanceObstacleDetected, float x, float y, float robotCoordinateXWorld, float robotCoordinateYWorld){
Ludwigfr 33:814bcd7d3cfe 16 float xSonar=robotCoordinateXWorld+this->distanceX;
Ludwigfr 33:814bcd7d3cfe 17 float ySonar=robotCoordinateYWorld+this->distanceY;
Ludwigfr 33:814bcd7d3cfe 18 float distancePointToSonar=sqrt(pow(x-xSonar,2)+pow(y-ySonar,2));
Ludwigfr 33:814bcd7d3cfe 19 if( distancePointToSonar < this->maxRange){
Ludwigfr 33:814bcd7d3cfe 20 float anglePointToSonar=this->compute_angle_between_vectors(x,y,xSonar,ySonar);//angle beetween the point and the sonar beam
Ludwigfr 33:814bcd7d3cfe 21 float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
Ludwigfr 33:814bcd7d3cfe 22 anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
Ludwigfr 33:814bcd7d3cfe 23
Ludwigfr 33:814bcd7d3cfe 24 if(alphaBeforeAdjustment>pi)
Ludwigfr 33:814bcd7d3cfe 25 alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
Ludwigfr 33:814bcd7d3cfe 26 if(alphaBeforeAdjustment<-pi)
Ludwigfr 33:814bcd7d3cfe 27 alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
Ludwigfr 33:814bcd7d3cfe 28
Ludwigfr 33:814bcd7d3cfe 29 //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
Ludwigfr 33:814bcd7d3cfe 30
Ludwigfr 33:814bcd7d3cfe 31 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
Ludwigfr 33:814bcd7d3cfe 32 //check if absolute difference between the angles is no more than Omega/2
Ludwigfr 33:814bcd7d3cfe 33 if(anglePointToSonar <= this->angleRange/2 || anglePointToSonar >= this->rad_angle_check(-this->angleRange/2)){
Ludwigfr 33:814bcd7d3cfe 34 if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){
Ludwigfr 33:814bcd7d3cfe 35 //point before obstacle, probably empty
Ludwigfr 33:814bcd7d3cfe 36 /*****************************************************************************/
Ludwigfr 33:814bcd7d3cfe 37 float Ea=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
Ludwigfr 33:814bcd7d3cfe 38 float Er;
Ludwigfr 33:814bcd7d3cfe 39 if(distancePointToSonar < this->minRange){
Ludwigfr 33:814bcd7d3cfe 40 //point before minimum sonar range
Ludwigfr 33:814bcd7d3cfe 41 Er=0.f;
Ludwigfr 33:814bcd7d3cfe 42 }else{
Ludwigfr 33:814bcd7d3cfe 43 //point after minimum sonar range
Ludwigfr 33:814bcd7d3cfe 44 Er=1.f-pow((distancePointToSonar-this->minRange)/(distanceObstacleDetected-this->incertitudeRange-this->minRange),2);
Ludwigfr 33:814bcd7d3cfe 45 }
Ludwigfr 33:814bcd7d3cfe 46 /*****************************************************************************/
Ludwigfr 33:814bcd7d3cfe 47 //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
Ludwigfr 33:814bcd7d3cfe 48 // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
Ludwigfr 33:814bcd7d3cfe 49 return (1.f-Er*Ea)/2.f;
Ludwigfr 33:814bcd7d3cfe 50 }else{
Ludwigfr 33:814bcd7d3cfe 51 //probably occupied
Ludwigfr 33:814bcd7d3cfe 52 /*****************************************************************************/
Ludwigfr 33:814bcd7d3cfe 53 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 33:814bcd7d3cfe 54 float Or;
Ludwigfr 33:814bcd7d3cfe 55 if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){
Ludwigfr 33:814bcd7d3cfe 56 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
Ludwigfr 33:814bcd7d3cfe 57 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(this->incertitudeRange),2);
Ludwigfr 33:814bcd7d3cfe 58 }else{
Ludwigfr 33:814bcd7d3cfe 59 //point after in range of the sonar but after the zone detected
Ludwigfr 33:814bcd7d3cfe 60 Or=0;
Ludwigfr 33:814bcd7d3cfe 61 }
Ludwigfr 33:814bcd7d3cfe 62 /*****************************************************************************/
Ludwigfr 33:814bcd7d3cfe 63 //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
Ludwigfr 33:814bcd7d3cfe 64 // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
Ludwigfr 33:814bcd7d3cfe 65 return (1+Or*Oa)/2;
Ludwigfr 33:814bcd7d3cfe 66 }
Ludwigfr 33:814bcd7d3cfe 67 }
Ludwigfr 33:814bcd7d3cfe 68 }
Ludwigfr 33:814bcd7d3cfe 69 //not checked by the sonar
Ludwigfr 33:814bcd7d3cfe 70 return 0.5;
Ludwigfr 33:814bcd7d3cfe 71 }
Ludwigfr 33:814bcd7d3cfe 72
Ludwigfr 33:814bcd7d3cfe 73 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 33:814bcd7d3cfe 74 float Sonar::compute_angle_between_vectors(float x, float y,float xs,float ys){
Ludwigfr 33:814bcd7d3cfe 75 //alpha angle between ->x and ->SA
Ludwigfr 33:814bcd7d3cfe 76 //vector S to A ->SA
Ludwigfr 33:814bcd7d3cfe 77 float vSAx=x-xs;
Ludwigfr 33:814bcd7d3cfe 78 float vSAy=y-ys;
Ludwigfr 33:814bcd7d3cfe 79 //norme SA
Ludwigfr 33:814bcd7d3cfe 80 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
Ludwigfr 33:814bcd7d3cfe 81 //vector ->x (1,0)
Ludwigfr 33:814bcd7d3cfe 82 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
Ludwigfr 33:814bcd7d3cfe 83 //vector ->y (0,1)
Ludwigfr 33:814bcd7d3cfe 84 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
Ludwigfr 33:814bcd7d3cfe 85 if (sinAlpha < 0)
Ludwigfr 33:814bcd7d3cfe 86 return -acos(cosAlpha);
Ludwigfr 33:814bcd7d3cfe 87 else
Ludwigfr 33:814bcd7d3cfe 88 return acos(cosAlpha);
Ludwigfr 33:814bcd7d3cfe 89 }
Ludwigfr 33:814bcd7d3cfe 90
Ludwigfr 33:814bcd7d3cfe 91 //makes the angle inAngle between 0 and 2pi
Ludwigfr 33:814bcd7d3cfe 92 float Sonar::rad_angle_check(float inAngle){
Ludwigfr 33:814bcd7d3cfe 93 //cout<<"before :"<<inAngle;
Ludwigfr 33:814bcd7d3cfe 94 if(inAngle > 0){
Ludwigfr 33:814bcd7d3cfe 95 while(inAngle > (2*pi))
Ludwigfr 33:814bcd7d3cfe 96 inAngle-=2*pi;
Ludwigfr 33:814bcd7d3cfe 97 }else{
Ludwigfr 33:814bcd7d3cfe 98 while(inAngle < 0)
Ludwigfr 33:814bcd7d3cfe 99 inAngle+=2*pi;
Ludwigfr 33:814bcd7d3cfe 100 }
Ludwigfr 33:814bcd7d3cfe 101 //cout<<" after :"<<inAngle<<endl;
Ludwigfr 33:814bcd7d3cfe 102 return inAngle;
Ludwigfr 33:814bcd7d3cfe 103 }
Ludwigfr 33:814bcd7d3cfe 104