Diff: main.cpp

Revision:

27:07bde633af72

Parent:

26:b020cf253059

Child:

28:f884979a02fa

--- a/main.cpp	Thu Apr 20 18:54:48 2017 +0000
+++ b/main.cpp	Mon Apr 24 11:55:55 2017 +0000
@@ -41,6 +41,8 @@
 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);
+void compute_angles_and_distance(float target_x, float target_y, float target_angle);
+void compute_linear_angular_velocities();
 
 const float pi=3.14159;
 //spec of the sonar
@@ -49,15 +51,14 @@
 const float RANGE_SONAR_MIN=10;//Rmin cm
 const float INCERTITUDE_SONAR=10;//cm
 const float ANGLE_SONAR=pi/3;//Omega rad
-const float SECURITY_DISTANCE=150;//mm
 
-//those distance and angle are approximation in need of measurements
+//those distance and angle are approximation in need of measurements, in the orthonormal frame
 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
-const float DISTANCE_SONAR_LEFT_X=4;
+const float DISTANCE_SONAR_LEFT_X=-4;
 const float DISTANCE_SONAR_LEFT_Y=4;
 
 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
-const float DISTANCE_SONAR_RIGHT_X=-4;
+const float DISTANCE_SONAR_RIGHT_X=4;
 const float DISTANCE_SONAR_RIGHT_Y=4;
 
 const float ANGLE_FRONT_TO_FRONT=0;
@@ -72,15 +73,15 @@
 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)
-const float DEFAULT_X=WIDTH_ARENA/2;
-const float DEFAULT_Y=HEIGHT_ARENA/2;
-const float DEFAULT_THETA=pi/2;
+//position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame
+const float DEFAULT_X=HEIGHT_ARENA/2;
+const float DEFAULT_Y=WIDTH_ARENA/2;
+const float DEFAULT_THETA=0;
 
 //used to create the map 250 represent the 250cm of the square where the robot is tested
 //float sizeCell=250/(float)SIZE_MAP;
-float sizeCellX=WIDTH_ARENA/(float)NB_CELL_WIDTH;
-float sizeCellY=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
+float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
+float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
 
 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
@@ -89,10 +90,7 @@
 const float RADIUS_WHEELS=3.25;
 const float DISTANCE_WHEELS=7.2;
 
-const int MAX_SPEED=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
-
-bool tooClose=false;
-bool turnFromObstacle=false;
+const int MAX_SPEED=250;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
 
 float alpha; //angle error
 float rho; //distance from target
@@ -121,14 +119,16 @@
     
     fill_initial_log_values();
 
-    theta=DEFAULT_THETA; 
+    theta=DEFAULT_THETA; //TODO
     X=DEFAULT_X;
     Y=DEFAULT_Y;
 
+    
     for (int i = 0; i<20; i++) {
         randomize_and_map();
         wait(2);
-        print_final_map();    
+        print_final_map();
+        //print_final_map_with_robot_position();    
     }
 
 }
@@ -149,8 +149,8 @@
 //generate a position randomly and makes the robot go there while updating the map
 void randomize_and_map() {
     //TODO check that it's aurelien's work
-    float target_x = (rand()%(int)(WIDTH_ARENA*10))/10;//for decimal precision
-    float target_y = (rand()%(int)(HEIGHT_ARENA*10))/10;
+    float target_x = (rand()%(int)(HEIGHT_ARENA*10))/10;//for decimal precision
+    float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
     float target_angle = ((float)(rand()%31416)-15708)/10000.0;
     
     //TODO comment that
@@ -164,24 +164,13 @@
 //go the the given position while updating the map
 //TODO clean this procedure it's ugly as hell and too long
 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
-    if(tooClose){
-        target_x=X;
-        target_y=Y;
-        target_angle=theta+pi/2;
-        target_angle = atan(sin(target_angle)/cos(target_angle));
-        pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", target_angle);
-    }
-        
     alpha = atan2((target_y-Y),(target_x-X))-theta;
     alpha = atan(sin(alpha)/cos(alpha));
     rho = dist(X, Y, target_x, target_y);
-    
     beta = -alpha-theta+target_angle;
     //beta = atan(sin(beta)/cos(beta));
-
+    bool keep_going=true;
     do {
-        //pc.printf("\n\n\r entered while");
-        
         //Timer stuff
         dt = t.read();
         t.reset();
@@ -189,7 +178,6 @@
         
         //Updating X,Y and theta with the odometry values
         Odometria();
-
         leftMm = get_distance_left_sensor();
         frontMm = get_distance_front_sensor();
         rightMm = get_distance_right_sensor();
@@ -197,51 +185,64 @@
         //pc.printf("\n\r leftMm=%f", leftMm);
         //pc.printf("\n\r frontMm=%f", frontMm);
         //pc.printf("\n\r rightMm=%f", rightMm);
-
-        update_sonar_values(leftMm, frontMm, rightMm);
-
-        if ((leftMm < SECURITY_DISTANCE || frontMm < SECURITY_DISTANCE || rightMm < SECURITY_DISTANCE) && turnFromObstacle==false) {
-            tooClose = true;
-            turnFromObstacle = true;
-            pc.printf("\n\r TOO CLOSE \n\r");
+    
+        //if in dangerzone 
+        if(frontMm < 100 || leftMm <100 || rightMm <100){
             leftMotor(1,0);
             rightMotor(1,0);
+            update_sonar_values(leftMm, frontMm, rightMm);
             Odometria();
-            go_to_point_with_angle(X, Y, rad_angle_check(theta+pi));
-            break;    
+            //do a flip TODO
+            keep_going=false;
+            target_angle=theta+pi;
+            target_y=Y;
+            target_x=X;
+            alpha = atan2((target_y-Y),(target_x-X))-theta;
+            alpha = atan(sin(alpha)/cos(alpha));
+            rho = dist(X, Y, target_x, target_y);
+            beta = -alpha-theta+target_angle;  
+            //keep going until the flip is done
+            do{
+                Odometria();
+                dt = t.read();
+                t.reset();
+                t.start();
+                compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
+                compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
+                leftMotor(1,angular_left);
+                rightMotor(1,angular_right);
+                //Timer stuff
+                t.stop();
+            }while(d2>1 && (abs(target_angle-theta)>0.01));       
+        }else{
+            //if not in danger zone continue as usual
+            update_sonar_values(leftMm, frontMm, rightMm);
+            compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
+            compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
+    
+            //pc.printf("\n\r X=%f", X);
+            //pc.printf("\n\r Y=%f", Y);
+    
+            //pc.printf("\n\r a_r=%f", angular_right);
+            //pc.printf("\n\r a_l=%f", angular_left);
+    
+            //Updating motor velocities
+            leftMotor(1,angular_left);
+            rightMotor(1,angular_right);
+    
+            wait(0.2);
+            //Timer stuff
+            t.stop();
         }
-        
-        compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
-        compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
-
-        //pc.printf("\n\r X=%f", X);
-        //pc.printf("\n\r Y=%f", Y);
-
-        pc.printf("\n\r a_r=%f", angular_right);
-        pc.printf("\n\r a_l=%f", angular_left);
-
-        //Updating motor velocities
-        leftMotor(1,angular_left);
-        rightMotor(1,angular_right);
-
-        wait(0.2);
-        //Timer stuff
-        t.stop();
-    } while(d2>1 && (abs(target_angle-theta)>0.01) && tooClose==false);
+    } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
 
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
-
-    if(turnFromObstacle){
-        turnFromObstacle=false;
-        tooClose=false;
-    }
 }
 
 //Updates sonar values
 void update_sonar_values(float leftMm, float frontMm, float rightMm){
-
     float currProba;
     float i_in_orthonormal;
     float j_in_orthonormal;
@@ -340,28 +341,28 @@
     float heightIndiceInOrthonormal;
     float widthIndiceInOrthonormal;
     
-    float heightMalus=-(3*sizeCellX/2);
-    float heightBonus=sizeCellX/2;
+    float widthMalus=-(3*sizeCellWidth/2);
+    float widthBonus=sizeCellWidth/2;
     
-    float widthMalus=-(3*sizeCellY/2);
-    float widthBonus=sizeCellY/2;
+    float heightMalus=-(3*sizeCellHeight/2);
+    float heightBonus=sizeCellHeight/2;
 
     pc.printf("\n\r");
     for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
         for (int x= 0; x<NB_CELL_WIDTH; x++) {
             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  ");
+            if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))                    
+                pc.printf(" R ");
             else{
                 currProba=log_to_proba(map[x][y]);
                 if ( currProba < 0.5)
-                    pc.printf("  0  ");
+                    pc.printf(" 0 ");
                 else{
                     if(currProba==0.5)
-                        pc.printf("  .  ");
+                        pc.printf(" . ");
                     else
-                        pc.printf("  +  ");
+                        pc.printf(" + ");
                 } 
             }
         }
@@ -369,7 +370,6 @@
     }
 }
 
-
 //MATHS heavy functions
 /**********************************************************************/
 //Distance computation function
@@ -425,39 +425,39 @@
 }
 
 float robot_center_x_in_orthonormal_x(){
-    return Y;
+    return NB_CELL_WIDTH*sizeCellWidth-Y;
 }
 
 float robot_center_y_in_orthonormal_y(){
-    return NB_CELL_WIDTH*sizeCellX-X;
+    return X;
 }
 
 float robot_sonar_front_x_in_orthonormal_x(){
-    return Y+DISTANCE_SONAR_FRONT_Y;
+    return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
 }
 float robot_sonar_front_y_in_orthonormal_y(){
-    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X;
+    return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
 }
 
 float robot_sonar_right_x_in_orthonormal_x(){
-    return Y+DISTANCE_SONAR_RIGHT_Y;
+    return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
 }
 float robot_sonar_right_y_in_orthonormal_y(){
-    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X;
+    return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
 }
 
 float robot_sonar_left_x_in_orthonormal_x(){
-    return Y+DISTANCE_SONAR_LEFT_Y;
+    return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
 }
 float robot_sonar_left_y_in_orthonormal_y(){
-    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X;
+    return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
 }
 
 float estimated_width_indice_in_orthonormal_x(int i){
-    return sizeCellX/2+i*sizeCellX;
+    return sizeCellWidth/2+i*sizeCellWidth;
 }
 float estimated_height_indice_in_orthonormal_y(int j){
-    return sizeCellY/2+j*sizeCellY;
+    return sizeCellHeight/2+j*sizeCellHeight;
 }
 
 void compute_angles_and_distance(float target_x, float target_y, float target_angle){
@@ -472,8 +472,8 @@
     temp = alpha;
     alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
     beta += dt*(-kRho*sin(temp));
-    pc.printf("\n\r d2=%f", d2);
-    pc.printf("\n\r dt=%f", dt);
+   //pc.printf("\n\r d2=%f", d2);
+    //pc.printf("\n\r dt=%f", dt);
 }
 
 void compute_linear_angular_velocities(){