Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 32:d51928b58645
- Parent:
- 31:352be78e1aad
- Child:
- 33:78139f82ea74
--- a/main.cpp Sat Apr 29 07:29:30 2017 +0000 +++ b/main.cpp Wed May 03 16:46:43 2017 +0000 @@ -45,8 +45,19 @@ float estimated_height_indice_in_orthonormal_y(int j); void compute_angles_and_distance(float target_x, float target_y, float target_angle); void compute_linear_angular_velocities(); +//update foceX and forceY if necessary +void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ); +//compute the X and Y force +void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY); + const float pi=3.14159; + +//CONSTANT FORCE FIELD +const float FORCE_CONSTANT_REPULSION=10;//TODO tweak it +const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it +const float RANGE_FORCE=50;//TODO tweak it + //spec of the sonar //TODO MEASURE THE DISTANCE on X and Y of the robot frame, between each sonar and the center of the robot and add it to calculus in updateSonarValues const float RANGE_SONAR=50;//cm @@ -68,12 +79,12 @@ const float DISTANCE_SONAR_FRONT_Y=5; //TODO adjust the size of the map for computation time (25*25?) -const float WIDTH_ARENA=250;//cm -const float HEIGHT_ARENA=250;//cm +const float WIDTH_ARENA=120;//cm +const float HEIGHT_ARENA=90;//cm //const int SIZE_MAP=25; -const int NB_CELL_WIDTH=20; -const int NB_CELL_HEIGHT=20; +const int NB_CELL_WIDTH=24; +const int NB_CELL_HEIGHT=18; //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame //this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position @@ -507,4 +518,38 @@ angular_left=speed*angular_left/angular_right; angular_right=speed; } +} + + +void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){ + + //get the coordonate of the map and the robot in the ortonormal frame + float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice); + float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice); + //compute the distance beetween the cell and the robot + float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2)); + //check if the cell is in range + if(distanceCellToRobot <= (range)) { + float probaCell=log_to_proba(map[widthIndice][heightIndice]); + float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3); + float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3); + *forceX+=xForceComputed; + *forceY+=yForceComputed; + } +} + +void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY){ + float xRobotOrtho=robot_center_x_in_orthonormal_x(); + float yRobotOrtho=robot_center_y_in_orthonormal_y(); + for(int i=0;i<NB_CELL_WIDTH;i++){ + for(int j=0;j<NB_CELL_HEIGHT;j++){ + updateForce(i,j,RANGE_FORCE,&forceX,&forceY,xRobotOrtho,yRobotOrtho); + } + } + //update with attraction force + forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-xRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2)); + forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-yRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2)); + float amplitude=sqrt(pow(forceX,2)+pow(forceY,2)); + forceX=forceX/amplitude; + forceY=forceY/amplitude; } \ No newline at end of file