Diff: main.cpp

Revision:

51:b863fad80574

Parent:

50:a970cf7889d3

Child:

52:54b3fe68a4f2

--- a/main.cpp	Tue May 30 17:18:55 2017 +0000
+++ b/main.cpp	Wed Jun 07 16:25:54 2017 +0000
@@ -4,6 +4,12 @@
 
 using namespace std;
 
+//update angularLeft and angularRight
+float get_error_angles(float x, float y,float theta);
+void test_procedure_Lab_2();
+void procedure_Lab_3();
+void procedure_Lab_2();
+void turn_to_target(float angleToTarget);
 void initialise_parameters();
 //fill initialLogValues with the values we already know (here the bordurs)
 void fill_initial_log_values();
@@ -13,6 +19,8 @@
 void do_half_flip();
 //go the the given position while updating the map
 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
+void compute_angles_and_distance(float target_x, float target_y, float target_angle);
+void compute_linear_angular_velocities();
 //Updates sonar values 
 void update_sonar_values(float leftMm, float frontMm, float rightMm);
 //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]
@@ -53,10 +61,6 @@
 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);
-//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);
-//update angularLeft and angularRight
-void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta, float* angularLeft, float* angularRight);
 //update foceX and forceY if necessary
 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
 //compute the X and Y force
@@ -70,6 +74,7 @@
 //set target in ortho space, in reference to the robot (so if the robot is in the middle, you want to him to go 10cm down and 15 right, set_target_to(15,-10)
 void set_target_to(float x, float y);
 void try_to_reach_target();
+void test_map_sonar();
 
 //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
@@ -103,12 +108,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;
+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;
@@ -124,25 +129,38 @@
 
 //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_X=20;
 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
+const int MAX_SPEED=50;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
 
 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
-const float FORCE_CONSTANT_ATTRACTION=25;//TODO tweak it
-const float RANGE_FORCE=50;//TODO tweak it
+const float FORCE_CONSTANT_REPULSION=10000;//TODO tweak it
+const float FORCE_CONSTANT_ATTRACTION=1;//TODO tweak it
+const float RANGE_FORCE=30;//TODO tweak it
+
+float alpha; //angle error
+float rho; //distance from target
+float beta;
+float linear, angular, angular_left, angular_right;
+float dt=0.5;
+float temp;
+float d2;
+Timer t;
 
 int main(){
     initialise_parameters();   
-    //try to reach the target     
-    set_target_to(50,-50);//
+    procedure_Lab_3();
+}
+
+void procedure_Lab_3(){
+    //try to reach the target is ortho space  
+    set_target_to(0,80);//
     print_final_map_with_robot_position_and_target();
     try_to_reach_target();
     //set_target_to(0,20);//lower right
@@ -157,21 +175,62 @@
     }
 }
 
+void test_map_sonar(){
+    float leftMm;
+    float frontMm;
+    float rightMm;
+    X=20;
+    Y=WIDTH_ARENA/2;
+    theta=0;
+    while(1){
+        leftMm = get_distance_left_sensor();
+        frontMm = get_distance_front_sensor();
+        rightMm = get_distance_right_sensor();
+        pc.printf("%f, %f, %f",leftMm,frontMm,rightMm);
+        //update the probabilities values 
+        update_sonar_values(leftMm, frontMm, rightMm);
+        print_final_map();
+    }
+}
+
+void test_procedure_Lab_2(){
+    X=HEIGHT_ARENA/2;
+    Y=WIDTH_ARENA/2;
+    set_target_to(-100,50);
+    print_final_map_with_robot_position_and_target();
+    go_to_point_with_angle(targetX, targetY, pi/2);
+    print_final_map_with_robot_position_and_target();
+    
+}
+void procedure_Lab_2(){
+    for(int i=0;i<25;i++){
+        randomize_and_map();
+        print_final_map_with_robot_position();
+        wait(2);
+    }
+}
+
+
 void try_to_reach_target(){
+    //atan2 gives the angle beetween pi and -pi
+    float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
+    turn_to_target(angleToTarget);
     bool reached=false;
     int print=0;
     while (!reached) {
         vff(&reached);
         //test_got_to_line(&reached);
-        if(print==30){
+        if(print==10){
             leftMotor(1,0);
             rightMotor(1,0);
+            /*
             pc.printf("\r\n theta=%f", theta);
             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);
+            */
             print_final_map_with_robot_position_and_target();
             print=0;
         }else
@@ -225,10 +284,20 @@
 //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)(HEIGHT_ARENA*10))/10;//for decimal precision
-    float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
+    float movementOnX=rand()%(int)(WIDTH_ARENA/2);
+    float movementOnY=rand()%(int)(HEIGHT_ARENA/2);
+    
+    float signOfX=rand()%2;
+    if(signOfX < 1)
+        signOfX=-1;
+    float signOfY=rand()%2;
+    if(signOfY  < 1)
+        signOfY=-1;
+        
+    float x = movementOnX*signOfX;
+    float y = movementOnY*signOfY;
     float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
-    
+    set_target_to(x,y);
     go_to_point_with_angle(targetX, targetY, target_angle);
 }
 
@@ -248,106 +317,6 @@
     rightMotor(1,0);    
 }
 
-//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) {
-    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);
-    float beta = -angleError-theta+target_angle;
-    //beta = atan(sin(beta)/cos(beta));
-    bool keep_going=true;
-    float leftMm;
-    float frontMm;
-    float rightMm;
-    float angularLeft=0; 
-    float angularRight=0;
-    Timer t;
-    float dt=0.5;//TODO better name please
-    float d2;//TODO better name please
-    do {
-        //Timer stuff
-        dt = t.read();
-        t.reset();
-        t.start();
-        
-        //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();
-
-        //pc.printf("\n\r leftMm=%f", leftMm);
-        //pc.printf("\n\r frontMm=%f", frontMm);
-        //pc.printf("\n\r rightMm=%f", rightMm);
-    
-        //if in dangerzone 
-        if(frontMm < 120 || leftMm <120 || rightMm <120){
-            leftMotor(1,0);
-            rightMotor(1,0);
-            update_sonar_values(leftMm, frontMm, rightMm);
-            //TODO Giorgos maybe you can also test the do_half_flip() function
-            Odometria();
-            //do a flip TODO
-            keep_going=false;
-            do_half_flip();   
-        }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,dt,&angleError,&distanceFromTarget,&d2,&beta);//Compute the angles and the distance from target
-            compute_linear_angular_velocities(angleError,distanceFromTarget,beta,&angularLeft,&angularRight); //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", angularRight);
-            //pc.printf("\n\r a_l=%f", angularLeft);
-    
-            //Updating motor velocities
-            leftMotor(sign2(angularLeft),abs(angularLeft));
-            rightMotor(sign2(angularRight),abs(angularRight));
-    
-            wait(0.2);
-            //Timer stuff
-            t.stop();
-        }
-    } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
-
-    //Stop at the end
-    leftMotor(1,0);
-    rightMotor(1,0);
-}
-
-//Updates sonar values
-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 space is within the range of the sonar (which has the coordinates xs, ys in the world space)
-            //check that again
-            //compute for front sonar
-            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);
-            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);
-            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);
-            if(currProba!=0.5)
-                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){
@@ -510,6 +479,7 @@
 /**********************************************************************/
 //Distance computation function
 float dist(float robot_x, float robot_y, float target_x, float target_y){
+    //pc.printf("%f, %f, %f, %f",robot_x,robot_y,target_x,target_y);
     return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
 }
 
@@ -542,7 +512,6 @@
     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
@@ -560,9 +529,24 @@
     else
         return acos(cosAlpha);
 }
-/*
+
+//return 1 if positiv, -1 if negativ
+float sign1(float value){
+    if(value>=0) 
+        return 1;
+    else 
+        return -1;
+}
 
+//return 1 if positiv, 0 if negativ
+int sign2(float value){
+    if(value>=0) 
+        return 1;
+    else 
+        return 0;
+}
 
+/*
 Robot space:      orthonormal space:
       ^                 ^
       |x                |y
@@ -615,53 +599,6 @@
     return sizeCellHeight/2+j*sizeCellHeight;
 }
 
-//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);
-    *d2 = *distanceFromTarget;
-    *beta = -*angleError-theta+target_angle;        
-    
-    //Computing angle error and distance towards the target value
-    *distanceFromTarget += dt*(-KRHO*cos(*angleError)**distanceFromTarget);
-    float temp = *angleError;
-    *angleError += dt*(KRHO*sin(*angleError)-KA**angleError-KB**beta);
-    *beta += dt*(-KRHO*sin(temp));
-   //pc.printf("\n\r d2=%f", d2);
-    //pc.printf("\n\r dt=%f", dt);
-}
-
-//update angularLeft and angularRight
-void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta,float* angularLeft, float* angularRight){
-    //Computing linear and angular velocities
-    float linear;
-    float angular;
-    if(angleError>=-1.5708 && angleError<=1.5708){
-        linear=KRHO*distanceFromTarget;
-        angular=KA*angleError+KB*beta;
-    }
-    else{
-        linear=-KRHO*distanceFromTarget;
-        angular=-KA*angleError-KB*beta;
-    }
-    //TODO check those signs
-    *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-    *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-    
-    float aL=*angularLeft;
-    float aR=*angularRight;
-    //Normalize speed for motors
-    if(abs(*angularLeft)>abs(*angularRight)) {  
-        *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR);
-        *angularLeft=MAX_SPEED*sign1(aL);
-    }
-    else {
-        *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL);
-        *angularRight=MAX_SPEED*sign1(aR);
-    }    
-}
-
 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
     //get the coordonate of the map and the robot in the ortonormal space
     float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
@@ -670,40 +607,17 @@
     float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
     //check if the cell is in range
     if(distanceCellToRobot <= range) {
-        float probaCell=log_to_proba(map[widthIndice][heightIndice]);
-        float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
-        float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
-        *forceX+=xForceComputed;
-        *forceY+=yForceComputed;
-    }
-}
-
-//compute the force on X and Y
-void compute_forceX_and_forceY(float* forceX, float* forceY){
-     //we put the position of the robot in an orthonormal space
-     float xRobotOrtho=robot_center_x_in_orthonormal_x();
-     float yRobotOrtho=robot_center_y_in_orthonormal_y();
-
-     float forceRepulsionComputedX=0;
-     float forceRepulsionComputedY=0;
-     //for each cell of the map we compute a force of repulsion
-     for(int i=0;i<NB_CELL_WIDTH;i++){
-        for(int j=0;j<NB_CELL_HEIGHT;j++){
-            update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
-        }
-    }
-    //update with attraction force
-    *forceX=-forceRepulsionComputedX;
-    *forceY=-forceRepulsionComputedY;
-    float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2));
-    if(distanceTargetRobot != 0){
-        *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot;
-        *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot;
-    }
-    float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
-    if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
-        *forceX=*forceX/amplitude;
-        *forceY=*forceY/amplitude;
+        /*float anglePointToRobot=compute_angle_between_vectors(xCenterCell,yCenterCell,xRobotOrtho,yRobotOrtho);//angle beetween the point and the sonar
+        float alphaBeforeAdjustment=anglePointToRobot-theta;
+        anglePointToRobot=rad_angle_check(alphaBeforeAdjustment);
+        if(anglePointToRobot <= pi/2 || anglePointToRobot >= rad_angle_check(-pi/2)){
+        */
+            float probaCell=log_to_proba(map[widthIndice][heightIndice]);
+            float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
+            float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
+            *forceX+=xForceComputed;
+            *forceY+=yForceComputed;
+        //}
     }
 }
 
@@ -763,105 +677,6 @@
         *reached=true;
 }
 
-//return 1 if positiv, -1 if negativ
-float sign1(float value){
-    if(value>=0) 
-        return 1;
-    else 
-        return -1;
-}
-
-//return 1 if positiv, 0 if negativ
-int sign2(float value){
-    if(value>=0) 
-        return 1;
-    else 
-        return 0;
-}
-
-//currently line_c is not used
-void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){
-    float lineAngle;
-    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));
-
-    //Calculating velocities
-    linear=KV*(3.1416);
-    angular=KH*angleError;
-    //TODO if we put it like the poly says it fails, if we switch the plus and minus it works ...
-    *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-    *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-    
-    float aL=*angularLeft;
-    float aR=*angularRight;
-    //Normalize speed for motors
-    if(abs(*angularLeft)>abs(*angularRight)) {  
-        *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR);
-        *angularLeft=MAX_SPEED*sign1(aL);
-    }
-    else {
-        *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL);
-        *angularRight=MAX_SPEED*sign1(aR);
-    }
-    pc.printf("\r\n line: %f x + %f y + %f =0 , X=%f; Y=%f", line_a, line_b, line_c,robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y());
-}
-
 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){
     /*
@@ -888,4 +703,287 @@
     //float yRobotOrtho=robot_center_y_in_orthonormal_y();
     //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho;    
     *line_c=0;
+}
+
+/*angleToTarget is obtained through atan2 so it s:
+< 0 if the angle is bettween pi and 2pi on a trigo circle
+> 0 if it is between 0 and pi
+*/
+void turn_to_target(float angleToTarget){
+    Odometria();
+    float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
+    if(theta_plus_h_pi > pi)
+        theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
+     if(angleToTarget>0){   
+        leftMotor(0,1);
+        rightMotor(1,1);
+    }else{
+        leftMotor(1,1);
+        rightMotor(0,1);
+    }
+    while(abs(angleToTarget-theta_plus_h_pi)>0.05){
+        Odometria();
+        theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
+         if(theta_plus_h_pi > pi)
+            theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
+        //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
+    }
+    leftMotor(1,0);
+    rightMotor(1,0);    
+}
+
+//currently line_c is not used
+void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){
+    float lineAngle;
+    float angleError;
+    float linear;
+    float angular; 
+    
+    bool direction=true;
+    
+    float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
+    bool inFront=true;
+    if(angleToTarget < 0)//the target is in front
+        inFront=false;
+
+    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
+        }
+    }
+    //pc.printf("\r\n Target is in front");
+    
+     if(line_b!=0){
+        if(!direction)
+            lineAngle=atan(-line_a/line_b);
+        else
+            lineAngle=atan(line_a/-line_b);
+    }
+    else{
+        lineAngle=1.5708;
+    }
+    
+    //Computing angle error
+    angleError = lineAngle-theta;
+    angleError = atan(sin(angleError)/cos(angleError));
+
+    //Calculating velocities
+    linear=KV*(3.1416);
+    angular=KH*angleError;
+
+    *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+    *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+    
+    float aL=*angularLeft;
+    float aR=*angularRight;
+    //Normalize speed for motors
+    if(abs(*angularLeft)>abs(*angularRight)) {  
+        *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR);
+        *angularLeft=MAX_SPEED*sign1(aL);
+    }
+    else {
+        *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL);
+        *angularRight=MAX_SPEED*sign1(aR);
+    }
+    pc.printf("\r\n X=%f; Y=%f", robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y());
+}
+
+//compute the force on X and Y
+void compute_forceX_and_forceY(float* forceX, float* forceY){
+     //we put the position of the robot in an orthonormal space
+     float xRobotOrtho=robot_center_x_in_orthonormal_x();
+     float yRobotOrtho=robot_center_y_in_orthonormal_y();
+
+     float forceRepulsionComputedX=0;
+     float forceRepulsionComputedY=0;
+     //for each cell of the map we compute a force of repulsion
+     for(int i=0;i<NB_CELL_WIDTH;i++){
+        for(int j=0;j<NB_CELL_HEIGHT;j++){
+            update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
+        }
+    }
+    //update with attraction force
+    *forceX=+forceRepulsionComputedX;
+    *forceY=+forceRepulsionComputedY;
+    float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2));
+    if(distanceTargetRobot != 0){
+        *forceX-=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot;
+        *forceY-=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot;
+    }
+    float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
+    if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
+        *forceX=*forceX/amplitude;
+        *forceY=*forceY/amplitude;
+    }
+}
+
+//x and y passed are TargetNotMap
+float get_error_angles(float x, float y,float theta){
+    float angleBeetweenRobotAndTarget=atan2(y,x);
+    if(y > 0){
+        if(angleBeetweenRobotAndTarget < pi/2)//up right
+            angleBeetweenRobotAndTarget=-pi/2+angleBeetweenRobotAndTarget;
+        else//up right
+            angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2;
+    }else{
+        if(angleBeetweenRobotAndTarget > -pi/2)//lower right
+            angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2;
+        else//lower left
+            angleBeetweenRobotAndTarget=2*pi+angleBeetweenRobotAndTarget-pi/2;
+    }
+    return angleBeetweenRobotAndTarget-theta;
+}
+
+void compute_angles_and_distance(float target_x, float target_y, float 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);
+    d2 = rho;
+    beta = -alpha-theta+target_angle;        
+    
+    //Computing angle error and distance towards the target value
+    rho += dt*(-KRHO*cos(alpha)*rho);
+    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);
+}
+
+void compute_linear_angular_velocities(){
+    //Computing linear and angular velocities
+    if(alpha>=-1.5708 && alpha<=1.5708){
+        linear=KRHO*rho;
+        angular=KA*alpha+KB*beta;
+    }
+    else{
+        linear=-KRHO*rho;
+        angular=-KA*alpha-KB*beta;
+    }
+    angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+    angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+    
+    //Normalize speed for motors
+    if(angular_left>angular_right) {
+        angular_right=MAX_SPEED*angular_right/angular_left;
+        angular_left=MAX_SPEED;
+    } else {
+        angular_left=MAX_SPEED*angular_left/angular_right;
+        angular_right=MAX_SPEED;
+    }
+}
+
+//go the the given position while updating the map
+void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
+    Odometria();
+    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;
+    float leftMm;
+    float frontMm; 
+    float rightMm;
+    do {
+        //Timer stuff
+        dt = t.read();
+        t.reset();
+        t.start();
+        
+        //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();
+
+        //pc.printf("\n\r leftMm=%f", leftMm);
+        //pc.printf("\n\r frontMm=%f", frontMm);
+        //pc.printf("\n\r rightMm=%f", rightMm);
+    
+        //if in dangerzone 
+        if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
+            leftMotor(1,0);
+            rightMotor(1,0);
+            update_sonar_values(leftMm, frontMm, rightMm);
+            //TODO Giorgos maybe you can also test the do_half_flip() function
+            Odometria();
+            //do a flip TODO
+            keep_going=false;
+            do_half_flip();   
+        }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();
+            pc.printf("\n\r dist to target= %f",d2);
+        }
+    } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+}
+
+//Updates sonar values
+void update_sonar_values(float leftMm, float frontMm, float rightMm){
+    if(leftMm==0)
+        pc.printf("\n\r leftMm==0");
+    if(frontMm==0)
+        pc.printf("\n\r frontMm==0");
+    if(rightMm==0)
+        pc.printf("\n\r rightMm==0");
+    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 space is within the range of the sonar (which has the coordinates xs, ys in the world space)
+            //check that again
+            //compute for front sonar
+            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(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);
+            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
+        }
+    }
 }
\ No newline at end of file