Diff: main.cpp

Revision:

38:3c9f8cbf5250

Parent:

37:462d19bb221f

Child:

39:dd8326ec75ce

--- a/main.cpp	Mon May 15 17:06:18 2017 +0000
+++ b/main.cpp	Thu May 18 13:44:31 2017 +0000
@@ -455,13 +455,23 @@
     else
         return acos(cosAlpha);
 }
+/*
 
+
+Robot frame:      orthonormal frame:
+      ^                 ^
+      |x                |y
+   <- R                 O ->
+    y                     x
+*/
+//Odometria must bu up to date
 float x_robot_in_orthonormal_x(float x, float y){
-    return NB_CELL_WIDTH*sizeCellWidth-y;
+    return robot_center_x_in_orthonormal_x()-y;
 }
 
+//Odometria must bu up to date
 float y_robot_in_orthonormal_y(float x, float y){
-    return x;
+    return robot_center_y_in_orthonormal_y()+x;
 }
 
 float robot_center_x_in_orthonormal_x(){
@@ -562,13 +572,17 @@
     }
 }
 
+//compute the force on X and Y
 void compute_forceX_and_forceY(float targetX, float targetY, float* forceX, float* forceY){
+     //we put the position of the robot in an orthonormal frame
      float xRobotOrtho=robot_center_x_in_orthonormal_x();
      float yRobotOrtho=robot_center_y_in_orthonormal_y();
+     //we put the target of the robot in an orthonormal frame
      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 each cell of the map we compute a force of repulsion
      for(int i=0;i<NB_CELL_WIDTH;i++){
         for(int j=0;j<NB_CELL_HEIGHT;j++){
             updateForce(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
@@ -596,14 +610,17 @@
     //Updating X,Y and theta with the odometry values
     float forceX=0;
     float forceY=0;
+    //we update the odometrie
     Odometria();
-    
+    //we check the sensors
     leftMm = get_distance_left_sensor();
     frontMm = get_distance_front_sensor();
     rightMm = get_distance_right_sensor();
+    //update the probabilities values 
     update_sonar_values(leftMm, frontMm, rightMm);
-    
+    //we compute the force on X and Y
     compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY);
+    //we compute a new ine
     calculate_line(forceX, forceY, X, Y);
     go_to_line();
     pc.printf("\r\n forceX=%f", forceX);