Yo Fleyt
with class
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Revision 37:b4c45e43ad29, committed 2017-06-11
- Comitter:
- Ludwigfr
- Date:
- Sun Jun 11 23:22:28 2017 +0000
- Parent:
- 36:b59d56d0b3b4
- Child:
- 38:5ed7c79fb724
- Commit message:
- changed a few thing, notably tried to simplify the mapping funciton
Changed in this revision
--- a/MiniExplorerCoimbra.cpp Sun Jun 11 22:53:59 2017 +0000
+++ b/MiniExplorerCoimbra.cpp Sun Jun 11 23:22:28 2017 +0000
@@ -3,7 +3,7 @@
#define PI 3.14159
-MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
+MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(PI/2+10*PI/36,-4,4),sonarFront(PI/2,0,5),sonarRight(PI/2-10*PI/36,4,4){
i2c1.frequency(100000);
initRobot(); //Initializing the robot
pc.baud(9600); // baud for the pc communication
@@ -171,9 +171,9 @@
xWorldCell=this->map.cell_width_coordinate_to_world(i);
yWorldCell=this->map.cell_height_coordinate_to_world(j);
//compute_probability_t(float distanceObstacleDetected, float x, float y, float[2] robotCoordinatesInWorld)
- this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,theta));
- this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,theta));
- this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,theta));
+ this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
+ this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,this->thetaWorld));
+ this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,this->thetaWorld));
}
}
@@ -192,7 +192,7 @@
float deplacementOnXWorld=targetXWorld-this->xWorld;
float deplacementOnYWorld=targetYWorld-this->yWorld;
*/
- float angleToTarget=atan2(targetXWorld-this->xWorld,targetYWorld-this->yWorld);
+ float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
turn_to_target(angleToTarget);
bool reached=false;
int print=0;
@@ -202,7 +202,7 @@
if(print==10){
leftMotor(1,0);
rightMotor(1,0);
- this->print_final_map_with_robot_position_and_target(targetXWorld,targetYWorld);
+ this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
print=0;
}else
print+=1;
@@ -314,7 +314,7 @@
float angular;
//atan2 use the deplacement on X and the deplacement on Y
- float angleToTarget=atan2(targetXWorld-this->xWorld,targetYWorld-this->yWorld);
+ float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
bool aligned=false;
//this condition is passed if the target is in the same direction as the robot orientation
@@ -404,7 +404,7 @@
rightMotor(1,0);
}
-void MiniExplorerCoimbra::print_final_map_with_robot_position() {
+void MiniExplorerCoimbra::print_map_with_robot_position() {
float currProba;
float heightIndiceInOrthonormal;
@@ -439,7 +439,7 @@
}
}
-void MiniExplorerCoimbra::print_final_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
+void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
float currProba;
float heightIndiceInOrthonormal;
@@ -478,7 +478,7 @@
}
}
-void MiniExplorerCoimbra::print_final_map() {
+void MiniExplorerCoimbra::print_map() {
float currProba;
pc.printf("\n\r");
for (int y = this->map.nbCellHeight -1; y>-1; y--) {
--- a/MiniExplorerCoimbra.hpp Sun Jun 11 22:53:59 2017 +0000 +++ b/MiniExplorerCoimbra.hpp Sun Jun 11 23:22:28 2017 +0000 @@ -55,11 +55,11 @@ //use virtual force field void try_to_reach_target(float targetXWorld,float targetYWorld); - void print_final_map_with_robot_position(); + void print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld); - void print_final_map_with_robot_position_and_target(float targetXWorld, float targetYWorld); + void print_map_with_robot_position(); - void print_final_map(); + void print_map(); private:
--- a/Sonar.cpp Sun Jun 11 22:53:59 2017 +0000
+++ b/Sonar.cpp Sun Jun 11 23:22:28 2017 +0000
@@ -2,8 +2,8 @@
#define PI 3.14159
-Sonar::Sonar(float angleFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){
- this->angleFromCenter=angleFromCenter;
+Sonar::Sonar(float angleFromOrigin, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){
+ this->angleFromOrigin=angleFromOrigin;
this->distanceX=distanceXFromRobotCenter;
this->distanceY=distanceYFromRobotCenter;
this->maxRange=50;//cm
@@ -12,31 +12,25 @@
this->angleRange=3.14159/3;//Omega rad
}
-//ODOMETRIA MUST HAVE BEEN CALLED
//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]
-float Sonar::compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float theta){
+float Sonar::compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float thetaWorld){
float xSonar=xRobotWorld+this->distanceX;
float ySonar=yRobotWorld+this->distanceY;
float distancePointToSonar=sqrt(pow(xCell-xSonar,2)+pow(yCell-ySonar,2));
+ //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
if( distancePointToSonar < this->maxRange){
- float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam
- float alphaBeforeAdjustment=anglePointToSonar-theta-this->angleFromCenter;
- anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
+ //float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam
+ float anglePointToSonar=atan2(yCell-yRobotWorld,xCell-xRobotWorld);//like world system
- if(alphaBeforeAdjustment>PI)
- alphaBeforeAdjustment=alphaBeforeAdjustment-2*PI;
- if(alphaBeforeAdjustment<-PI)
- alphaBeforeAdjustment=alphaBeforeAdjustment+2*PI;
+ float angleOriginToMidleOfBeam=thetaWorld+this->angleFromOrigin;//
- //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
-
- //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
+ float angleDifference=anglePointToSonar-angleOriginToMidleOfBeam;
//check if absolute difference between the angles is no more than Omega/2
- if(anglePointToSonar <= this->angleRange/2 || anglePointToSonar >= this->rad_angle_check(-this->angleRange/2)){
+ if(abs(angleDifference) <= this->angleRange/2){
if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){
//point before obstacle, probably empty
/*****************************************************************************/
- float Ea=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
+ float Ea=1.f-pow((2*angleDifference)/this->angleRange,2);
float Er;
if(distancePointToSonar < this->minRange){
//point before minimum sonar range
@@ -47,12 +41,12 @@
}
/*****************************************************************************/
//if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
- // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
+ // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,angleDifference=%f",(1.f-Er*Ea)/2.f,Er,Ea,angleDifference);
return (1.f-Er*Ea)/2.f;
}else{
//probably occuPIed
/*****************************************************************************/
- float Oa=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
+ float Oa=1.f-pow((2*angleDifference)/this->angleRange,2);
float Or;
if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){
//point between distanceObstacleDetected +- INCERTITUDE_SONAR
@@ -63,7 +57,7 @@
}
/*****************************************************************************/
//if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
- // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
+ // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,angleDifference=%f",(1+Or*Oa)/2,Or,Oa,angleDifference);
return (1+Or*Oa)/2;
}
}
--- a/Sonar.hpp Sun Jun 11 22:53:59 2017 +0000 +++ b/Sonar.hpp Sun Jun 11 23:22:28 2017 +0000 @@ -10,12 +10,12 @@ float minRange;//Rmin cm float incertitudeRange;//cm float angleRange;//Omega rad - float angleFromCenter; + float angleFromOrigin; float distanceX; float distanceY; //the distance are in the world coordinates - Sonar(float anlgeFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ); + Sonar(float angleFromOrigin, float distanceXFromRobotCenter, float distanceYFromRobotCenter ); float compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float theta);