Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

Sonar.cpp@14:696187e74411, 2017-07-12 (annotated)

Committer:
Ludwigfr
Date:
Wed Jul 12 18:08:07 2017 +0000
Revision:
14:696187e74411
Parent:
13:b0ddd71e8f08 
lol

Who changed what in which revision?

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