Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 37:462d19bb221f
- Parent:
- 36:c806c568720a
- Child:
- 38:3c9f8cbf5250
diff -r c806c568720a -r 462d19bb221f main.cpp --- a/main.cpp Mon May 15 16:42:59 2017 +0000 +++ b/main.cpp Mon May 15 17:06:18 2017 +0000 @@ -562,19 +562,21 @@ } } -void compute_forceX_and_forceY(float targetX, float targetY,float* forceX, float* forceY){ +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(); 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(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); + updateForce(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); } } //update with attraction force - *forceX=-*forceX; - *forceY=-*forceY; + *forceX=-forceRepulsionComputedX; + *forceY=-forceRepulsionComputedY; *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrtho-xRobotOrtho)/sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2)); *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrtho-yRobotOrtho)/sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2)); float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); @@ -592,7 +594,8 @@ void vff(){ //Updating X,Y and theta with the odometry values - float forceX, forceY; + float forceX=0; + float forceY=0; Odometria(); leftMm = get_distance_left_sensor(); @@ -600,7 +603,6 @@ rightMm = get_distance_right_sensor(); update_sonar_values(leftMm, frontMm, rightMm); - updateForce(WIDTH_ARENA, HEIGHT_ARENA, RANGE_FORCE, &forceX, &forceY, robot_center_x_in_orthonormal_x(), robot_center_y_in_orthonormal_y()); //TODO check range value, I randomly put 20 compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY); calculate_line(forceX, forceY, X, Y); go_to_line();