Diff: main.cpp

Revision:

47:f0fe58571e4a

Parent:

45:fb07065a64a9

Child:

48:cd3463dcca04

--- a/main.cpp	Mon May 29 12:59:51 2017 +0000
+++ b/main.cpp	Tue May 30 15:17:19 2017 +0000
@@ -94,20 +94,12 @@
 const float DISTANCE_SONAR_FRONT_Y=5;
 
 //TODO adjust the size of the map for computation time (25*25?)
-const float WIDTH_ARENA=120;//cm
-const float HEIGHT_ARENA=90;//cm
+const float WIDTH_ARENA=200;//cm
+const float HEIGHT_ARENA=200;//cm
 
 //const int SIZE_MAP=25;
-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) it's in the robot space
-//this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position
-//const float DEFAULT_X=HEIGHT_ARENA/2;
-//const float DEFAULT_Y=WIDTH_ARENA/2;
-const float DEFAULT_X=20;//lower right
-const float DEFAULT_Y=20;//lower right
-const float DEFAULT_THETA=0;
+const int NB_CELL_WIDTH=20;
+const int NB_CELL_HEIGHT=20;
 
 //used to create the map 250 represent the 250cm of the square where the robot is tested
 //float sizeCell=250/(float)SIZE_MAP;
@@ -121,10 +113,18 @@
 const float RADIUS_WHEELS=3.25;
 const float DISTANCE_WHEELS=7.2;
 
+//position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space
+//this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position
+const float DEFAULT_X=HEIGHT_ARENA/2;
+const float DEFAULT_Y=WIDTH_ARENA/2;
+//const float DEFAULT_X=20;//lower right
+//const float DEFAULT_Y=20;//lower right
+const float DEFAULT_THETA=0;
+
 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=200, KH=200; //Kappa values
 
 //CONSTANT FORCE FIELD
 const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it
@@ -134,21 +134,24 @@
 //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     
-    set_target_to(0,50);//up right
+    set_target_to(-50,-50);//
     print_final_map_with_robot_position_and_target();
     try_to_reach_target();
-    set_target_to(0,-50);//lower right
-    print_final_map_with_robot_position_and_target();
-    try_to_reach_target();
-    set_target_to(-50,50);//up left
-    print_final_map_with_robot_position_and_target();
-    try_to_reach_target();
+    //set_target_to(0,20);//lower right
+    //print_final_map_with_robot_position_and_target();
+    //try_to_reach_target();
+    //set_target_to(-20,-20);//up left
+    //print_final_map_with_robot_position_and_target();
+    //try_to_reach_target();
     //print the map forever
     while(1){
          print_final_map_with_robot_position_and_target();
@@ -179,6 +182,7 @@
     leftMotor(1,0);
     rightMotor(1,0);
     pc.printf("\r\n target reached");
+    wait(3);//
 }
 
 //target in ortho space
@@ -187,6 +191,8 @@
     targetY=-x;
     targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY);
     targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY);
+    targetXOrhtoNotMap=x;
+    targetYOrhtoNotMap=y;
 }
 
 void initialise_parameters(){
@@ -365,6 +371,8 @@
                 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{
             //probably occupied
@@ -379,6 +387,8 @@
                 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{
@@ -522,6 +532,7 @@
     return 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){
     //alpha angle between ->x and ->SA
@@ -764,13 +775,58 @@
     float angleError;
     float linear;
     float angular;
+    
+    bool inFront=true;
+    //atan2 gives the angle beetween pi and -pi
+    float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
+    pc.printf("angleToTarget=%f",angleToTarget);
+    if(angleToTarget < 0)//the target is in front
+        inFront=false;
+
+    bool direction=true;
+    
+    if(theta > 0){
+        //the robot is oriented to the left
+        if(theta > pi/2){
+            //the robot is oriented lower left
+            if(inFront)
+                direction=false;//robot and target not oriented the same way
+        }else{
+            //the robot is oriented upper left
+            if(!inFront)
+                direction=false;//robot and target not oriented the same way
+        }
+    }else{
+        //the robot is oriented to the right
+        if(theta < -pi/2){
+            //the robot is oriented lower right
+            if(inFront)
+                direction=false;//robot and target not oriented the same way
+        }else{ 
+            //the robot is oriented upper right
+            if(!inFront)
+                direction=false;//robot and target not oriented the same way
+        }
+    }
+  
+     if(line_b!=0){
+        if(!direction)
+            lineAngle=atan(-line_a/line_b);
+        else
+            lineAngle=atan(line_a/-line_b);
+    }
+    else{
+        lineAngle=1.5708;
+    }
+
+    /*
     if(line_b!=0){
         lineAngle=atan(-line_a/line_b);
     }
     else{
         lineAngle=1.5708;
     }
-    
+    */
     //Computing angle error
     angleError = lineAngle-theta;
     angleError = atan(sin(angleError)/cos(angleError));