Diff: main.cpp

Revision:

25:572c9e9a8809

Parent:

24:8f4b820d8de8

Child:

26:b020cf253059

--- a/main.cpp	Wed Apr 19 22:20:56 2017 +0000
+++ b/main.cpp	Thu Apr 20 15:02:28 2017 +0000
@@ -16,6 +16,8 @@
 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
 //print the map
 void print_final_map();
+//print the map with the robot marked on it
+void print_final_map_with_robot_position();
 
 //MATHS heavy functions
 float dist(float robot_x, float robot_y, float target_x, float target_y);
@@ -29,6 +31,16 @@
 float rad_angle_check(float inAngle);
 //returns the angle between the vectors (x,y) and (xs,ys)
 float compute_angle_between_vectors(float x, float y,float xs,float ys);
+float robot_center_x_in_orthonormal_x();
+float robot_center_y_in_orthonormal_y();
+float robot_sonar_front_x_in_orthonormal_x();
+float robot_sonar_front_y_in_orthonormal_y();
+float robot_sonar_right_x_in_orthonormal_x();
+float robot_sonar_right_y_in_orthonormal_y();
+float robot_sonar_left_x_in_orthonormal_x();
+float robot_sonar_left_y_in_orthonormal_y();
+float estimated_width_indice_in_orthonormal_x(int i);
+float estimated_height_indice_in_orthonormal_y(int j);
 
 const float pi=3.14159;
 //spec of the sonar
@@ -272,23 +284,29 @@
 void update_sonar_values(float leftMm, float frontMm, float rightMm){
 
     float currProba;
+    float i_in_orthonormal;
+    float j_in_orthonormal;
     for(int i=0;i<NB_CELL_WIDTH;i++){
         for(int j=0;j<NB_CELL_HEIGHT;j++){
                 //check if the point A(x,y) in the world frame is within the range of the sonar (which has the coordinates xs, ys in the world frame)
             //check that again
             //compute for front sonar
-                currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_FRONT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X,ANGLE_FRONT_TO_FRONT,frontMm/10);
+            i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
+            j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
+
+            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_front_x_in_orthonormal_x(),robot_sonar_front_y_in_orthonormal_y(),ANGLE_FRONT_TO_FRONT,frontMm/10);
             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];//map is filled as map[0][0] get the data for the point closest to the origin
             //compute for right sonar
-                currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_RIGHT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X,ANGLE_FRONT_TO_RIGHT,rightMm/10);
+            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_right_x_in_orthonormal_x(),robot_sonar_right_y_in_orthonormal_y(),ANGLE_FRONT_TO_RIGHT,rightMm/10);
             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
              //compute for left sonar
-                currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_LEFT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X,ANGLE_FRONT_TO_LEFT,leftMm/10);
+            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_left_x_in_orthonormal_x(),robot_sonar_left_y_in_orthonormal_y(),ANGLE_FRONT_TO_LEFT,leftMm/10);
             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
         }
     }
 }
 
+//ODOMETRIA MUST HAVE BEEN CALLED
 //function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
 
@@ -330,12 +348,12 @@
             return (1+Or*Oa)/2;
         }
     }else{
-        //not in range of the sonar
+        //not checked by the sonar
         return 0.5;
     }
 }
 
-void print_final_map_1() {
+void print_final_map() {
     float currProba;
     pc.printf("\n\r");
     for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
@@ -354,25 +372,39 @@
     }
 }
 
-void print_final_map_2() {
+void print_final_map_with_robot_position() {
     float currProba;
+    Odometria();
+    float Xrobot=robot_center_x_in_orthonormal_x();
+    float Yrobot=robot_center_y_in_orthonormal_y();
+    
+    float heightIndiceInOrthonormal;
+    float widthIndiceInOrthonormal;
+    
+    float heightMalus=-(3*sizeCellX/2);
+    float heightBonus=sizeCellX/2;
+    
+    float widthMalus=-(3*sizeCellY/2);
+    float widthBonus=sizeCellY/2;
+
     pc.printf("\n\r");
     for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
         for (int x= 0; x<NB_CELL_WIDTH; x++) {
-            currProba=log_to_proba(map[x][y]);
-            if(x => && x <= && y=> && y<= ){
+            heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
+            widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
+            if(Xrobot >= (heightIndiceInOrthonormal+heightMalus) && Xrobot <= (heightIndiceInOrthonormal+heightBonus) && Yrobot >= (widthIndiceInOrthonormal+widthMalus) && Yrobot <= (widthIndiceInOrthonormal+widthBonus))                    
                 pc.printf("  R  ");
-            }else{
-               if ( currProba < 0.5) {
-                pc.printf("  0  ");
-            } else {
-                if(currProba==0.5)
-                    pc.printf("  .  ");
-                else
-                    pc.printf("  +  ");
-            } 
+            else{
+                currProba=log_to_proba(map[x][y]);
+                if ( currProba < 0.5)
+                    pc.printf("  0  ");
+                else{
+                    if(currProba==0.5)
+                        pc.printf("  .  ");
+                    else
+                        pc.printf("  +  ");
+                } 
             }
-            
         }
         pc.printf("\n\r");
     }
@@ -431,4 +463,40 @@
         return -acos(cosAlpha);
     else
         return acos(cosAlpha);
+}
+
+float robot_center_x_in_orthonormal_x(){
+    return Y;
+}
+
+float robot_center_y_in_orthonormal_y(){
+    return NB_CELL_WIDTH*sizeCellX-X;
+}
+
+float robot_sonar_front_x_in_orthonormal_x(){
+    return Y+DISTANCE_SONAR_FRONT_Y;
+}
+float robot_sonar_front_y_in_orthonormal_y(){
+    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X;
+}
+
+float robot_sonar_right_x_in_orthonormal_x(){
+    return Y+DISTANCE_SONAR_RIGHT_Y;
+}
+float robot_sonar_right_y_in_orthonormal_y(){
+    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X;
+}
+
+float robot_sonar_left_x_in_orthonormal_x(){
+    return Y+DISTANCE_SONAR_LEFT_Y;
+}
+float robot_sonar_left_y_in_orthonormal_y(){
+    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X;
+}
+
+float estimated_width_indice_in_orthonormal_x(int i){
+    return sizeCellX/2+i*sizeCellX;
+}
+float estimated_height_indice_in_orthonormal_y(int j){
+    return sizeCellY/2+j*sizeCellY;
 }
\ No newline at end of file