Diff: main.cpp

Revision:

37:462d19bb221f

Parent:

36:c806c568720a

Child:

38:3c9f8cbf5250

--- 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();