with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 34:c208497dd079
- Parent:
- 33:814bcd7d3cfe
- Child:
- 35:68f9edbb3cff
diff -r 814bcd7d3cfe -r c208497dd079 MiniExplorerCoimbra.cpp --- a/MiniExplorerCoimbra.cpp Fri Jun 09 00:28:32 2017 +0000 +++ b/MiniExplorerCoimbra.cpp Fri Jun 09 14:30:21 2017 +0000 @@ -1,33 +1,34 @@ #include "MiniExplorerCoimbra.hpp" +#include "robot.h" #define PI 3.14159 -MiniExplorerCoimbra:: MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){ - i2c1.frequency(100000); +MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){ + i2c1.frequency(100000); initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication measure_always_on();//TODO check if needed - this->setXYTheta(defaultX,defaultY,defaultTheta); - this->radiusWheels=3.25; - this->distanceWheels=7.2; - - this->khro=12; - this->ka=30; - this->kb=-13; - this->kv=200; - this->kh=200; + this->setXYTheta(defaultX,defaultY,defaultTheta); + this->radiusWheels=3.25; + this->distanceWheels=7.2; + + this->khro=12; + this->ka=30; + this->kb=-13; + this->kv=200; + this->kh=200; - this->rangeForce=30; - this->attractionConstantForce=10000; - this->repulsionConstantForce=1; + this->rangeForce=30; + this->attractionConstantForce=10000; + this->repulsionConstantForce=1; } void MiniExplorerCoimbra::setXYTheta(float x, float y, float theta){ - X=x; - Y=y; - theta=theta; + X=x; + Y=y; + theta=theta; } //generate a position randomly and makes the robot go there while updating the map @@ -102,14 +103,16 @@ void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){ float xWorldCell; float yWorldCell; + float xRobotWorld=this->get_converted_robot_X_into_world(); + float yRobotWorld=this->get_converted_robot_Y_into_world(); for(int i=0;i<this->map.nbCellWidth;i++){ for(int j=0;j<this->map.nbCellHeight;j++){ 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->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world())); - this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world())); - this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world())); + this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,xRobotWorld,yRobotWorld,theta)); + this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,xRobotWorld,yWorldCell,theta)); + this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,xRobotWorld,yWorldCell,theta)); } } @@ -210,7 +213,7 @@ pc.printf("\r\n X Target=%f", targetXWorld); pc.printf("\r\n Y Target=%f", targetYWorld); */ - print_final_map_with_robot_position_and_target(); + this->print_final_map_with_robot_position_and_target(X,Y,targetXWorld,targetYWorld); print=0; }else print+=1; @@ -456,3 +459,101 @@ return angleBeetweenRobotAndTarget-theta; } */ + + +void MiniExplorerCoimbra::print_final_map_with_robot_position(float robot_x,float robot_y) { + float currProba; + float Xrobot=this->map.robot_x_coordinate_in_world(robot_x,robot_y); + float Yrobot=this->map.robot_y_coordinate_in_world(robot_x,robot_y); + + float heightIndiceInOrthonormal; + float widthIndiceInOrthonormal; + + float widthMalus=-(3*this->map.sizeCellWidth/2); + float widthBonus=this->map.sizeCellWidth/2; + + float heightMalus=-(3*this->map.sizeCellHeight/2); + float heightBonus=this->map.sizeCellHeight/2; + + pc.printf("\n\r"); + for (int y = this->map.nbCellHeight -1; y>-1; y--) { + for (int x= 0; x<this->map.nbCellWidth; x++) { + heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y); + widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x); + if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" R "); + else{ + currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]); + if ( currProba < 0.5) + pc.printf(" "); + else{ + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + } + pc.printf("\n\r"); + } +} + +void MiniExplorerCoimbra::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) { + float currProba; + float Xrobot=this->map.robot_x_coordinate_in_world(robot_x,robot_y); + float Yrobot=this->map.robot_y_coordinate_in_world(robot_x,robot_y); + + float heightIndiceInOrthonormal; + float widthIndiceInOrthonormal; + + float widthMalus=-(3*this->map.sizeCellWidth/2); + float widthBonus=this->map.sizeCellWidth/2; + + float heightMalus=-(3*this->map.sizeCellHeight/2); + float heightBonus=this->map.sizeCellHeight/2; + + pc.printf("\n\r"); + for (int y = this->map.nbCellHeight -1; y>-1; y--) { + for (int x= 0; x<this->map.nbCellWidth; x++) { + heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y); + widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x); + if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" R "); + else{ + if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" T "); + else{ + currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]); + if ( currProba < 0.5) + pc.printf(" "); + else{ + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + } + } + pc.printf("\n\r"); + } +} + +void MiniExplorerCoimbra::print_final_map() { + float currProba; + pc.printf("\n\r"); + for (int y = this->map.nbCellHeight -1; y>-1; y--) { + for (int x= 0; x<this->map.nbCellWidth; x++) { + currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]); + if ( currProba < 0.5) { + pc.printf(" "); + } else { + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + pc.printf("\n\r"); + } +}