All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 36:c806c568720a
- Parent:
- 35:c8f224ab153f
- Child:
- 37:462d19bb221f
diff -r c8f224ab153f -r c806c568720a main.cpp --- a/main.cpp Mon May 15 15:49:26 2017 +0000 +++ b/main.cpp Mon May 15 16:42:59 2017 +0000 @@ -456,6 +456,14 @@ return acos(cosAlpha); } +float x_robot_in_orthonormal_x(float x, float y){ + return NB_CELL_WIDTH*sizeCellWidth-y; +} + +float y_robot_in_orthonormal_y(float x, float y){ + return x; +} + float robot_center_x_in_orthonormal_x(){ return NB_CELL_WIDTH*sizeCellWidth-Y; } @@ -557,6 +565,8 @@ 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); 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); @@ -565,11 +575,13 @@ //update with attraction force *forceX=-*forceX; *forceY=-*forceY; - *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-X)/sqrt(pow(targetX-X,2)+pow(targetY-Y,2)); - *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-Y)/sqrt(pow(targetX-X,2)+pow(targetY-Y,2)); + *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)); - *forceX=*forceX/amplitude; - *forceY=*forceY/amplitude; + if(amplitude!=0){//avoid division by 0 + *forceX=*forceX/amplitude; + *forceY=*forceY/amplitude; + } } void calculate_line(float forceX, float forceY, float robotX, float robotY){