Diff: main.cpp

Revision:

36:c806c568720a

Parent:

35:c8f224ab153f

Child:

37:462d19bb221f

--- 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){