with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Committer:
Ludwigfr
Date:
Thu Jun 15 23:17:55 2017 +0000
Revision:
38:5ed7c79fb724
Parent:
37:b4c45e43ad29
with some part of lab 4

Who changed what in which revision?

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