with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
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?
User | Revision | Line number | New 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 |