Revision 46:e57ebcf747dc, committed 2017-05-29

Comitter:

geotsam

Date:

Mon May 29 17:13:42 2017 +0000

Parent:

45:fb07065a64a9

Commit message:

full of isnan() and other weird debugging stuff

Changed in this revision

--- a/main.cpp	Mon May 29 12:59:51 2017 +0000
+++ b/main.cpp	Mon May 29 17:13:42 2017 +0000
@@ -4,6 +4,8 @@
 
 using namespace std;
 
+//makes the angle inAngle between pi and minus pi
+float rad_angle_check_pi_and_minus_pi(float inAngle);
 void initialise_parameters();
 //fill initialLogValues with the values we already know (here the bordurs)
 void fill_initial_log_values();
@@ -124,18 +126,20 @@
 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=300, KH=150; //Kappa values
+const float KRHO=12, KA=30, KB=-13, KV=150, KH=150; //Kappa values
 
 //CONSTANT FORCE FIELD
-const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it
+const float FORCE_CONSTANT_REPULSION=80;//TODO tweak it
 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)
+//those target are in comparison 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
-float targetXOrhto;
-float targetYOrhto;
+float targetXOrtho;
+float targetYOrtho;
+
+bool direction;
 
 int main(){
     initialise_parameters();   
@@ -168,8 +172,8 @@
             pc.printf("\r\n IN ORTHO:");
             pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x());
             pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y());
-            pc.printf("\r\n X Target=%f", targetXOrhto);
-            pc.printf("\r\n Y Target=%f", targetYOrhto);
+            pc.printf("\r\n X Target=%f", targetXOrtho);
+            pc.printf("\r\n Y Target=%f", targetYOrtho);
             print_final_map_with_robot_position_and_target();
             print=0;
         }else
@@ -185,8 +189,17 @@
 void set_target_to(float x, float y){
     targetX=y;
     targetY=-x;
-    targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY);
-    targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY);
+    targetXOrtho=x_robot_in_orthonormal_x(targetX,targetY);
+    targetYOrtho=y_robot_in_orthonormal_y(targetX,targetY);
+    
+    pc.printf("\r\nangletarget= %f", atan2(x,y));
+    float angleError = atan2(x,y)-theta;
+    if(angleError>pi) angleError-=2*pi;
+    if(angleError<-pi) angleError+=2*pi;
+        
+    if(angleError<(pi/2) && angleError>(-pi/2)) direction=true;
+    else direction=false;
+    pc.printf("\r\nangleError= %f", angleError);
 }
 
 void initialise_parameters(){
@@ -249,8 +262,12 @@
     set_target_to(target_x,target_y);
     Odometria();
     float angleError = atan2((target_y-Y),(target_x-X))-theta;
-    angleError = atan(sin(angleError)/cos(angleError));
-    float distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto);
+    if(!cos(angleError))
+        angleError = atan(sin(angleError)/cos(angleError));
+    else
+        angleError=pi/2;
+    if(isnan(angleError)) pc.printf("\r\n nan line 264");
+    float distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho);
     float beta = -angleError-theta+target_angle;
     //beta = atan(sin(beta)/cos(beta));
     bool keep_going=true;
@@ -329,17 +346,34 @@
             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);
+            if(isnan(currProba)) pc.printf("\r\n currProba is nan");
             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];
              //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);
+            if(isnan(currProba)) pc.printf("\r\n nan line 354");
             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
+            if(isnan(proba_to_log(currProba))) pc.printf("\r\nnan in line 355");
         }
     }
 }
 
+//makes the angle inAngle between pi and minus pi
+float rad_angle_check_pi_and_minus_pi(float inAngle){
+    //cout<<"before :"<<inAngle;
+    if(inAngle > 0){
+        while(inAngle > (pi))
+            inAngle-=pi;
+    }else{
+        while(inAngle < 0)
+            inAngle+=pi;
+    }
+    //cout<<" after :"<<inAngle<<endl;
+    return inAngle;
+}
+
 //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){
@@ -347,6 +381,8 @@
     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
+    alphaBeforeAdjustment=rad_angle_check_pi_and_minus_pi(alphaBeforeAdjustment);
+    //if(abs(alphaBeforeAdjustment)>ANGLE_SONAR/2) pc.printf("\r\n it is!");
     float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
 
     //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
@@ -365,6 +401,8 @@
                 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
             }
          /*****************************************************************************/
+            if((1.f-Er*Ea)/2.f>1) pc.printf("\r\n E>1");
+            if((1.f-Er*Ea)/2.f<0) pc.printf("\r\n E<0");
             return (1.f-Er*Ea)/2.f;
         }else{
             //probably occupied
@@ -379,12 +417,13 @@
                 Or=0;
             }
         /*****************************************************************************/
+            if((1+Or*Oa)/2>1) pc.printf("\r\n O>1");
+            if((1+Or*Oa)/2<0) pc.printf("\r\n O<0");
             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() {
@@ -467,7 +506,7 @@
             if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
                 pc.printf(" R ");
             else{
-                if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus))                    
+                if(targetYOrtho >= (heightIndiceInOrthonormal+heightMalus) && targetYOrtho <= (heightIndiceInOrthonormal+heightBonus) && targetXOrtho >= (widthIndiceInOrthonormal+widthMalus) && targetXOrtho <= (widthIndiceInOrthonormal+widthBonus))                    
                     pc.printf(" T ");
                 else{
                     currProba=log_to_proba(map[x][y]);
@@ -493,14 +532,23 @@
     return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
 }
 
-//returns the probability [0,1] that the cell is occupied from the log valAue lt
+//returns the probability [0,1] that the cell is occupied from the log value lt
 float log_to_proba(float lt){
-    return 1-1/(1+exp(lt));
+    float temp=1-1/(1+exp(lt));
+    if(isnan(temp)){
+        //pc.printf("\r\n nan in line 514");
+        //pc.printf("\r\nlt= %f, 1+exp(lt)= %f", lt, 1+exp(lt));
+    }
+    return temp;
 }
 
 //returns the log value that the cell is occupied from the probability value [0,1]
 float proba_to_log(float p){
-    return log(p/(1-p));
+    float temp;
+    if(p==1) temp=log(0.99/(1-0.99));
+    else temp=log(p/(1-p));
+    if(isnan(temp)) pc.printf("\r\n temp=%f, p=%f", temp,p);
+    return temp;
 }
 
 //returns the new log value
@@ -597,8 +645,12 @@
 //update angleError,distanceFromTarget,d2, beta
 void compute_angles_and_distance(float target_x, float target_y, float target_angle,float dt,float* angleError,float* distanceFromTarget,float* d2,float* beta){
     *angleError = atan2((target_y-Y),(target_x-X))-theta;
-    *angleError = atan(sin(*angleError)/cos(*angleError));
-    *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto);
+    if(!cos(*angleError))
+        *angleError = atan(sin(*angleError)/cos(*angleError));
+    else
+        *angleError=pi/2;
+    if(isnan(*angleError)) pc.printf("\r\n nan line 613");
+    *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho);
     *d2 = *distanceFromTarget;
     *beta = -*angleError-theta+target_angle;        
     
@@ -650,8 +702,11 @@
     //check if the cell is in range
     if(distanceCellToRobot <= range) {
         float probaCell=log_to_proba(map[widthIndice][heightIndice]);
+        //if(isnan(probaCell)) pc.printf("\r\nnan in probaCell");
         float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
+        //if(isnan(xForceComputed)) pc.printf("\r\nnan in line 673");
         float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
+        //if(isnan(yForceComputed)) pc.printf("\r\nnan in line 675");
         *forceX+=xForceComputed;
         *forceY+=yForceComputed;
     }
@@ -674,10 +729,10 @@
     //update with attraction force
     *forceX=-forceRepulsionComputedX;
     *forceY=-forceRepulsionComputedY;
-    float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2));
+    float distanceTargetRobot=sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2));
     if(distanceTargetRobot != 0){
-        *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot;
-        *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot;
+        *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrtho-xRobotOrtho)/distanceTargetRobot;
+        *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrtho-yRobotOrtho)/distanceTargetRobot;
     }
     float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
     if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
@@ -701,17 +756,17 @@
     leftMotor(sign2(angularLeft),abs(angularLeft));
     rightMotor(sign2(angularRight),abs(angularRight));
     
-    pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto));
+    pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho));
 
     //wait(0.1);
     Odometria();
-    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
+    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)<10)
         *reached=true;
 }
 void vff(bool* reached){
-    float line_a;
-    float line_b;
-    float line_c;
+    float line_a=0;
+    float line_b=0;
+    float line_c=0;
     //Updating X,Y and theta with the odometry values
     float forceX=0;
     float forceY=0;
@@ -738,7 +793,7 @@
 
     //wait(0.1);
     Odometria();
-    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
+    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)<10)
         *reached=true;
 }
 
@@ -764,8 +819,12 @@
     float angleError;
     float linear;
     float angular;
+    
     if(line_b!=0){
-        lineAngle=atan(-line_a/line_b);
+        if(direction)
+            lineAngle=atan(-line_a/line_b);
+        else
+            lineAngle=atan(line_a/-line_b);
     }
     else{
         lineAngle=1.5708;
@@ -773,7 +832,11 @@
     
     //Computing angle error
     angleError = lineAngle-theta;
-    angleError = atan(sin(angleError)/cos(angleError));
+    if(!cos(angleError))
+        angleError = atan(sin(angleError)/cos(angleError));
+    else
+        angleError=pi/2;
+    if(isnan(angleError)) pc.printf("\r\n nan line 794");
 
     //Calculating velocities
     linear=KV*(3.1416);