test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
Sonar.cpp@2:11cd5173aa36, 2017-07-05 (annotated)
- Committer:
- Ludwigfr
- Date:
- Wed Jul 05 08:56:12 2017 +0000
- Revision:
- 2:11cd5173aa36
- Parent:
- 0:9f7ee7ed13e4
- Child:
- 3:37345c109dfc
test 05 07 9:56
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ludwigfr | 0:9f7ee7ed13e4 | 1 | #include "Sonar.hpp" |
Ludwigfr | 0:9f7ee7ed13e4 | 2 | |
Ludwigfr | 0:9f7ee7ed13e4 | 3 | #define PI 3.14159 |
Ludwigfr | 0:9f7ee7ed13e4 | 4 | |
Ludwigfr | 0:9f7ee7ed13e4 | 5 | Sonar::Sonar(float angleFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){ |
Ludwigfr | 0:9f7ee7ed13e4 | 6 | this->angleFromCenter=angleFromCenter; |
Ludwigfr | 0:9f7ee7ed13e4 | 7 | this->distanceX=distanceXFromRobotCenter; |
Ludwigfr | 0:9f7ee7ed13e4 | 8 | this->distanceY=distanceYFromRobotCenter; |
Ludwigfr | 0:9f7ee7ed13e4 | 9 | this->maxRange=50;//cm |
Ludwigfr | 0:9f7ee7ed13e4 | 10 | this->minRange=10;//Rmin cm |
Ludwigfr | 0:9f7ee7ed13e4 | 11 | this->incertitudeRange=10;//cm |
Ludwigfr | 0:9f7ee7ed13e4 | 12 | this->angleRange=3.14159/3;//Omega rad |
Ludwigfr | 0:9f7ee7ed13e4 | 13 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 14 | |
Ludwigfr | 0:9f7ee7ed13e4 | 15 | //return distance sonar to cell if in range, -1 if not |
Ludwigfr | 0:9f7ee7ed13e4 | 16 | float Sonar::isInRange(float xCell, float yCell, float xRobotWorld, float yRobotWorld, float thetaWorld){ |
Ludwigfr | 0:9f7ee7ed13e4 | 17 | float xSonar=xRobotWorld+this->distanceX; |
Ludwigfr | 0:9f7ee7ed13e4 | 18 | float ySonar=yRobotWorld+this->distanceY; |
Ludwigfr | 0:9f7ee7ed13e4 | 19 | float distanceCellToSonar=sqrt(pow(xCell-xSonar,2)+pow(yCell-ySonar,2)); |
Ludwigfr | 0:9f7ee7ed13e4 | 20 | |
Ludwigfr | 0:9f7ee7ed13e4 | 21 | //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS |
Ludwigfr | 0:9f7ee7ed13e4 | 22 | if( distanceCellToSonar < this->maxRange){ |
Ludwigfr | 0:9f7ee7ed13e4 | 23 | //float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam |
Ludwigfr | 0:9f7ee7ed13e4 | 24 | float angleCellToSonar=atan2(yCell-yRobotWorld,xCell-xRobotWorld);//like world system |
Ludwigfr | 0:9f7ee7ed13e4 | 25 | |
Ludwigfr | 0:9f7ee7ed13e4 | 26 | float angleOriginToMidleOfBeam=thetaWorld+this->angleFromCenter;// |
Ludwigfr | 0:9f7ee7ed13e4 | 27 | |
Ludwigfr | 0:9f7ee7ed13e4 | 28 | float angleDifference=angleCellToSonar-angleOriginToMidleOfBeam; |
Ludwigfr | 0:9f7ee7ed13e4 | 29 | if(angleDifference > PI) |
Ludwigfr | 0:9f7ee7ed13e4 | 30 | angleDifference=angleDifference-2*PI; |
Ludwigfr | 0:9f7ee7ed13e4 | 31 | if(angleDifference < -PI) |
Ludwigfr | 0:9f7ee7ed13e4 | 32 | angleDifference=angleDifference+2*PI; |
Ludwigfr | 0:9f7ee7ed13e4 | 33 | //check if absolute difference between the angles is no more than Omega/2 |
Ludwigfr | 0:9f7ee7ed13e4 | 34 | if(angleDifference > 0 && angleDifference <= this->angleRange/2 ||angleDifference < 0 && angleDifference >= -this->angleRange/2 ){ |
Ludwigfr | 0:9f7ee7ed13e4 | 35 | return distanceCellToSonar; |
Ludwigfr | 0:9f7ee7ed13e4 | 36 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 37 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 38 | return -1; |
Ludwigfr | 0:9f7ee7ed13e4 | 39 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 40 | |
Ludwigfr | 0:9f7ee7ed13e4 | 41 | //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 | 0:9f7ee7ed13e4 | 42 | float Sonar::compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float thetaWorld){ |
Ludwigfr | 0:9f7ee7ed13e4 | 43 | float xSonar=xRobotWorld+this->distanceX; |
Ludwigfr | 0:9f7ee7ed13e4 | 44 | float ySonar=yRobotWorld+this->distanceY; |
Ludwigfr | 0:9f7ee7ed13e4 | 45 | float distancePointToSonar=sqrt(pow(xCell-xSonar,2)+pow(yCell-ySonar,2)); |
Ludwigfr | 0:9f7ee7ed13e4 | 46 | //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS |
Ludwigfr | 0:9f7ee7ed13e4 | 47 | if( distancePointToSonar < this->maxRange){ |
Ludwigfr | 0:9f7ee7ed13e4 | 48 | //float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam |
Ludwigfr | 0:9f7ee7ed13e4 | 49 | float anglePointToSonar=atan2(yCell-yRobotWorld,xCell-xRobotWorld);//like world system |
Ludwigfr | 0:9f7ee7ed13e4 | 50 | |
Ludwigfr | 0:9f7ee7ed13e4 | 51 | float angleOriginToMidleOfBeam=thetaWorld+this->angleFromCenter;// |
Ludwigfr | 0:9f7ee7ed13e4 | 52 | |
Ludwigfr | 0:9f7ee7ed13e4 | 53 | float angleDifference=anglePointToSonar-angleOriginToMidleOfBeam; |
Ludwigfr | 0:9f7ee7ed13e4 | 54 | if(angleDifference > PI) |
Ludwigfr | 0:9f7ee7ed13e4 | 55 | angleDifference=angleDifference-2*PI; |
Ludwigfr | 0:9f7ee7ed13e4 | 56 | if(angleDifference < -PI) |
Ludwigfr | 0:9f7ee7ed13e4 | 57 | angleDifference=angleDifference+2*PI; |
Ludwigfr | 0:9f7ee7ed13e4 | 58 | //check if absolute difference between the angles is no more than Omega/2 |
Ludwigfr | 0:9f7ee7ed13e4 | 59 | if(angleDifference > 0 && angleDifference <= this->angleRange/2 ||angleDifference < 0 && angleDifference >= -this->angleRange/2 ){ |
Ludwigfr | 0:9f7ee7ed13e4 | 60 | |
Ludwigfr | 0:9f7ee7ed13e4 | 61 | if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){ |
Ludwigfr | 0:9f7ee7ed13e4 | 62 | //point before obstacle, probably empty |
Ludwigfr | 0:9f7ee7ed13e4 | 63 | /*****************************************************************************/ |
Ludwigfr | 0:9f7ee7ed13e4 | 64 | float Ea=1.f-pow((2*angleDifference)/this->angleRange,2); |
Ludwigfr | 0:9f7ee7ed13e4 | 65 | float Er; |
Ludwigfr | 0:9f7ee7ed13e4 | 66 | if(distancePointToSonar < this->minRange){ |
Ludwigfr | 0:9f7ee7ed13e4 | 67 | //point before minimum sonar range |
Ludwigfr | 0:9f7ee7ed13e4 | 68 | Er=0.f; |
Ludwigfr | 0:9f7ee7ed13e4 | 69 | }else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 70 | //point after minimum sonar range |
Ludwigfr | 0:9f7ee7ed13e4 | 71 | Er=1.f-pow((distancePointToSonar-this->minRange)/(distanceObstacleDetected-this->incertitudeRange-this->minRange),2); |
Ludwigfr | 0:9f7ee7ed13e4 | 72 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 73 | /*****************************************************************************/ |
Ludwigfr | 0:9f7ee7ed13e4 | 74 | //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0) |
Ludwigfr | 2:11cd5173aa36 | 75 | //pc.printf("\n\r return value=%f,Er=%f,Ea=%f,angleDifference=%f",(1.f-Er*Ea)/2.f,Er,Ea,angleDifference); |
Ludwigfr | 0:9f7ee7ed13e4 | 76 | return (1.f-Er*Ea)/2.f; |
Ludwigfr | 0:9f7ee7ed13e4 | 77 | }else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 78 | //probably occuPIed |
Ludwigfr | 0:9f7ee7ed13e4 | 79 | /*****************************************************************************/ |
Ludwigfr | 0:9f7ee7ed13e4 | 80 | float Oa=1.f-pow((2*angleDifference)/this->angleRange,2); |
Ludwigfr | 0:9f7ee7ed13e4 | 81 | float Or; |
Ludwigfr | 0:9f7ee7ed13e4 | 82 | if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){ |
Ludwigfr | 0:9f7ee7ed13e4 | 83 | //point between distanceObstacleDetected +- INCERTITUDE_SONAR |
Ludwigfr | 0:9f7ee7ed13e4 | 84 | Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(this->incertitudeRange),2); |
Ludwigfr | 0:9f7ee7ed13e4 | 85 | }else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 86 | //point after in range of the sonar but after the zone detected |
Ludwigfr | 0:9f7ee7ed13e4 | 87 | Or=0; |
Ludwigfr | 0:9f7ee7ed13e4 | 88 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 89 | /*****************************************************************************/ |
Ludwigfr | 0:9f7ee7ed13e4 | 90 | //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0) |
Ludwigfr | 2:11cd5173aa36 | 91 | //pc.printf("\n\r return value=%f,Er=%f,Ea=%f,angleDifference=%f",(1+Or*Oa)/2,Or,Oa,angleDifference); |
Ludwigfr | 0:9f7ee7ed13e4 | 92 | return (1+Or*Oa)/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 93 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 94 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 95 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 96 | //not checked by the sonar |
Ludwigfr | 0:9f7ee7ed13e4 | 97 | return 0.5; |
Ludwigfr | 0:9f7ee7ed13e4 | 98 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 99 | |
Ludwigfr | 0:9f7ee7ed13e4 | 100 | //returns the angle between the vectors (x,y) and (xs,ys) |
Ludwigfr | 0:9f7ee7ed13e4 | 101 | float Sonar::compute_angle_between_vectors(float x, float y,float xs,float ys){ |
Ludwigfr | 0:9f7ee7ed13e4 | 102 | //alpha angle between ->x and ->SA |
Ludwigfr | 0:9f7ee7ed13e4 | 103 | //vector S to A ->SA |
Ludwigfr | 0:9f7ee7ed13e4 | 104 | float vSAx=x-xs; |
Ludwigfr | 0:9f7ee7ed13e4 | 105 | float vSAy=y-ys; |
Ludwigfr | 0:9f7ee7ed13e4 | 106 | //norme SA |
Ludwigfr | 0:9f7ee7ed13e4 | 107 | float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2)); |
Ludwigfr | 0:9f7ee7ed13e4 | 108 | //vector ->x (1,0) |
Ludwigfr | 0:9f7ee7ed13e4 | 109 | float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;; |
Ludwigfr | 0:9f7ee7ed13e4 | 110 | //vector ->y (0,1) |
Ludwigfr | 0:9f7ee7ed13e4 | 111 | float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx; |
Ludwigfr | 0:9f7ee7ed13e4 | 112 | if (sinAlpha < 0) |
Ludwigfr | 0:9f7ee7ed13e4 | 113 | return -acos(cosAlpha); |
Ludwigfr | 0:9f7ee7ed13e4 | 114 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 115 | return acos(cosAlpha); |
Ludwigfr | 0:9f7ee7ed13e4 | 116 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 117 | |
Ludwigfr | 0:9f7ee7ed13e4 | 118 | //makes the angle inAngle between 0 and 2PI |
Ludwigfr | 0:9f7ee7ed13e4 | 119 | float Sonar::rad_angle_check(float inAngle){ |
Ludwigfr | 0:9f7ee7ed13e4 | 120 | if(inAngle > 0){ |
Ludwigfr | 0:9f7ee7ed13e4 | 121 | while(inAngle > (2*PI)) |
Ludwigfr | 0:9f7ee7ed13e4 | 122 | inAngle-=2*PI; |
Ludwigfr | 0:9f7ee7ed13e4 | 123 | }else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 124 | while(inAngle < 0) |
Ludwigfr | 0:9f7ee7ed13e4 | 125 | inAngle+=2*PI; |
Ludwigfr | 0:9f7ee7ed13e4 | 126 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 127 | return inAngle; |
Ludwigfr | 0:9f7ee7ed13e4 | 128 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 129 |