Diff: main.cpp

Revision:

50:a970cf7889d3

Parent:

49:d61da2bc8882

Child:

51:b863fad80574

--- a/main.cpp	Tue May 30 16:09:25 2017 +0000
+++ b/main.cpp	Tue May 30 17:18:55 2017 +0000
@@ -71,6 +71,15 @@
 void set_target_to(float x, float y);
 void try_to_reach_target();
 
+//those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move)
+float targetX;//this is in the robot space top left
+float targetY;//this is in the robot space top left
+//Ortho but for the map (i.e x and Y are > 0)
+float targetXOrhto;
+float targetYOrhto;
+float targetXOrhtoNotMap;
+float targetYOrhtoNotMap;
+
 const float pi=3.14159;
 
 //spec of the sonar
@@ -94,12 +103,12 @@
 const float DISTANCE_SONAR_FRONT_Y=5;
 
 //TODO adjust the size of the map for computation time (25*25?)
-const float WIDTH_ARENA=200;//cm
-const float HEIGHT_ARENA=200;//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;
 
 //used to create the map 250 represent the 250cm of the square where the robot is tested
 //float sizeCell=250/(float)SIZE_MAP;
@@ -123,7 +132,6 @@
 
 const int MAX_SPEED=200;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
 
-//TODO all those global variables are making me sad
 const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values
 
 //CONSTANT FORCE FIELD
@@ -131,15 +139,6 @@
 const float FORCE_CONSTANT_ATTRACTION=25;//TODO tweak it
 const float RANGE_FORCE=50;//TODO tweak it
 
-//those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move)
-float targetX;//this is in the robot space top left
-float targetY;//this is in the robot space top left
-//Ortho but for the map (i.e x and Y are > 0)
-float targetXOrhto;
-float targetYOrhto;
-float targetXOrhtoNotMap;
-float targetYOrhtoNotMap;
-
 int main(){
     initialise_parameters();   
     //try to reach the target     
@@ -164,7 +163,7 @@
     while (!reached) {
         vff(&reached);
         //test_got_to_line(&reached);
-        if(print==10){
+        if(print==30){
             leftMotor(1,0);
             rightMotor(1,0);
             pc.printf("\r\n theta=%f", theta);
@@ -335,13 +334,16 @@
             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
+            if(currProba!=0.5)
+                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(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];
+            if(currProba!=0.5)
+                map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
              //compute for left sonar
             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];
+            if(currProba!=0.5)
+                map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
         }
     }
 }
@@ -350,58 +352,59 @@
 //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){
 
-    float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
-    float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
-    anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
     float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
-    
-    if(alphaBeforeAdjustment>pi)
-        alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
-    if(alphaBeforeAdjustment<-pi)
-        alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
+    if( distancePointToSonar < RANGE_SONAR){
+        float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
+        float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
+        anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
+        
+        if(alphaBeforeAdjustment>pi)
+            alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
+        if(alphaBeforeAdjustment<-pi)
+            alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
+            
+        //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
         
-    //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
-    
-    //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
-    //check if absolute difference between the angles is no more than Omega/2
-    if( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2))){
-        if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
-        //point before obstacle, probably empty
-        /*****************************************************************************/
-            float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
-            float Er;
-            if(distancePointToSonar < RANGE_SONAR_MIN){
-                //point before minimum sonar range
-                Er=0.f;
+        //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
+        //check if absolute difference between the angles is no more than Omega/2
+        if(anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2)){
+            if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
+            //point before obstacle, probably empty
+            /*****************************************************************************/
+                float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
+                float Er;
+                if(distancePointToSonar < RANGE_SONAR_MIN){
+                    //point before minimum sonar range
+                    Er=0.f;
+                }else{
+                    //point after minimum sonar range
+                    Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
+                }
+             /*****************************************************************************/
+                //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
+                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
+                return (1.f-Er*Ea)/2.f;
             }else{
-                //point after minimum sonar range
-                Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
+                //probably occupied
+            /*****************************************************************************/
+                float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
+                float Or;
+                if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
+                    //point between distanceObstacleDetected +- INCERTITUDE_SONAR
+                    Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
+                }else{
+                    //point after in range of the sonar but after the zone detected
+                    Or=0;
+                }
+            /*****************************************************************************/
+                //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
+                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
+                return (1+Or*Oa)/2;
             }
-         /*****************************************************************************/
-            if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
-                pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
-            return (1.f-Er*Ea)/2.f;
-        }else{
-            //probably occupied
-        /*****************************************************************************/
-            float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
-            float Or;
-            if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
-                //point between distanceObstacleDetected +- INCERTITUDE_SONAR
-                Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
-            }else{
-                //point after in range of the sonar but after the zone detected
-                Or=0;
-            }
-        /*****************************************************************************/
-            if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
-                pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
-            return (1+Or*Oa)/2;
-        }
-    }else{
-        //not checked by the sonar
-        return 0.5;
+        }   
     }
+    //not checked by the sonar
+    return 0.5;
 }
 
 void print_final_map() {