Diff: main.cpp

Revision:

32:d51928b58645

Parent:

31:352be78e1aad

Child:

33:78139f82ea74

--- a/main.cpp	Sat Apr 29 07:29:30 2017 +0000
+++ b/main.cpp	Wed May 03 16:46:43 2017 +0000
@@ -45,8 +45,19 @@
 float estimated_height_indice_in_orthonormal_y(int j);
 void compute_angles_and_distance(float target_x, float target_y, float target_angle);
 void compute_linear_angular_velocities();
+//update foceX and forceY if necessary
+void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
+//compute the X and Y force
+void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY);
+
 
 const float pi=3.14159;
+
+//CONSTANT FORCE FIELD
+const float FORCE_CONSTANT_REPULSION=10;//TODO tweak it
+const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it
+const float RANGE_FORCE=50;//TODO tweak it
+
 //spec of the sonar
 //TODO MEASURE THE DISTANCE on X and Y of the robot frame, between each sonar and the center of the robot and add it to calculus in updateSonarValues
 const float RANGE_SONAR=50;//cm
@@ -68,12 +79,12 @@
 const float DISTANCE_SONAR_FRONT_Y=5;
 
 //TODO adjust the size of the map for computation time (25*25?)
-const float WIDTH_ARENA=250;//cm
-const float HEIGHT_ARENA=250;//cm
+const float WIDTH_ARENA=120;//cm
+const float HEIGHT_ARENA=90;//cm
 
 //const int SIZE_MAP=25;
-const int NB_CELL_WIDTH=20;
-const int NB_CELL_HEIGHT=20;
+const int NB_CELL_WIDTH=24;
+const int NB_CELL_HEIGHT=18;
 
 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame
 //this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position
@@ -507,4 +518,38 @@
         angular_left=speed*angular_left/angular_right;
         angular_right=speed;
     }
+}
+
+
+void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
+    
+    //get the coordonate of the map and the robot in the ortonormal frame
+    float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
+    float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
+    //compute the distance beetween the cell and the robot
+    float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
+    //check if the cell is in range
+    if(distanceCellToRobot <= (range)) {
+        float probaCell=log_to_proba(map[widthIndice][heightIndice]);
+        float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
+        float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
+        *forceX+=xForceComputed;
+        *forceY+=yForceComputed;
+    }
+}
+
+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();
+     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);
+        }
+    }
+    //update with attraction force  
+    forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-xRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
+    forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-yRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
+    float amplitude=sqrt(pow(forceX,2)+pow(forceY,2));
+    forceX=forceX/amplitude;
+    forceY=forceY/amplitude;
 }
\ No newline at end of file