Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 38:3c9f8cbf5250
- Parent:
- 37:462d19bb221f
- Child:
- 39:dd8326ec75ce
--- a/main.cpp Mon May 15 17:06:18 2017 +0000 +++ b/main.cpp Thu May 18 13:44:31 2017 +0000 @@ -455,13 +455,23 @@ else return acos(cosAlpha); } +/* + +Robot frame: orthonormal frame: + ^ ^ + |x |y + <- R O -> + y x +*/ +//Odometria must bu up to date float x_robot_in_orthonormal_x(float x, float y){ - return NB_CELL_WIDTH*sizeCellWidth-y; + return robot_center_x_in_orthonormal_x()-y; } +//Odometria must bu up to date float y_robot_in_orthonormal_y(float x, float y){ - return x; + return robot_center_y_in_orthonormal_y()+x; } float robot_center_x_in_orthonormal_x(){ @@ -562,13 +572,17 @@ } } +//compute the force on X and Y void compute_forceX_and_forceY(float targetX, float targetY, float* forceX, float* forceY){ + //we put the position of the robot in an orthonormal frame float xRobotOrtho=robot_center_x_in_orthonormal_x(); float yRobotOrtho=robot_center_y_in_orthonormal_y(); + //we put the target of the robot in an orthonormal frame float targetXOrtho=x_robot_in_orthonormal_x(targetX,targetY); float targetYOrtho=y_robot_in_orthonormal_y(targetX,targetY); float forceRepulsionComputedX=0; float forceRepulsionComputedY=0; + //for each cell of the map we compute a force of repulsion for(int i=0;i<NB_CELL_WIDTH;i++){ for(int j=0;j<NB_CELL_HEIGHT;j++){ updateForce(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); @@ -596,14 +610,17 @@ //Updating X,Y and theta with the odometry values float forceX=0; float forceY=0; + //we update the odometrie Odometria(); - + //we check the sensors leftMm = get_distance_left_sensor(); frontMm = get_distance_front_sensor(); rightMm = get_distance_right_sensor(); + //update the probabilities values update_sonar_values(leftMm, frontMm, rightMm); - + //we compute the force on X and Y compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY); + //we compute a new ine calculate_line(forceX, forceY, X, Y); go_to_line(); pc.printf("\r\n forceX=%f", forceX);