Diff: main.cpp

Revision:

43:ffd5a6d4dd48

Parent:

42:ab25bffdc32b

Child:

44:e2bd06f94dc0

--- a/main.cpp	Mon May 22 13:25:20 2017 +0000
+++ b/main.cpp	Mon May 22 18:20:08 2017 +0000
@@ -13,7 +13,7 @@
 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);
-//Updates sonar values
+//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]
 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
@@ -27,6 +27,7 @@
 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c);
 //calculate virtual force field and move
 void vff(bool* reached);
+void test_got_to_line(bool* reached);
 
 //MATHS heavy functions
 float dist(float robot_x, float robot_y, float target_x, float target_y);
@@ -57,19 +58,19 @@
 //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 updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
+void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
 //compute the X and Y force
 void compute_forceX_and_forceY(float* forceX, float* forceY);
 //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);
+//return 1 if positiv, -1 if negativ
+float sign1(float value);
+//return 1 if positiv, 0 if negativ
+int sign2(float value);
+void set_target_to(float x, float y);
 
 const float pi=3.14159;
 
-//CONSTANT FORCE FIELD
-const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it
-const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it
-const float RANGE_FORCE=50;//TODO tweak it
-
 //spec of the sonar
 //TODO MEASURE THE DISTANCE on X and Y of the robot frame, between each sonar and the center of the robot and add it to calculus in updateSonarValues
 const float RANGE_SONAR=50;//cm
@@ -121,13 +122,18 @@
 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
+const float KRHO=12, KA=30, KB=-13, KV=300, KH=150; //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
 
 //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)
-const float targetX=HEIGHT_ARENA-X-20;//this is in the robot frame top left
-const float targetY=WIDTH_ARENA+Y-20;//this is in the robot frame top left
-float targetX_orhto=0;
-float targetY_orhto=0;
+float targetX;//this is in the robot frame top left
+float targetY;//this is in the robot frame top left
+float targetXOrhto;
+float targetYOrhto;
 
 int main(){
     initialise_parameters();
@@ -135,7 +141,8 @@
    //try to reach the target     
    bool reached=false; 
     while (!reached) {
-        vff(&reached); 
+        vff(&reached);
+        //test_got_to_line(&reached);
         print_final_map_with_robot_position_and_target();
     }
     //Stop at the end
@@ -149,6 +156,13 @@
     
 }
 
+void set_target_to(float x, float y){
+    targetX=x;
+    targetY=y;
+    targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY);
+    targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY);
+}
+
 void initialise_parameters(){
     i2c1.frequency(100000);
     initRobot(); //Initializing the robot
@@ -162,8 +176,7 @@
     theta=DEFAULT_THETA;
     X=DEFAULT_X;
     Y=DEFAULT_Y;
-    targetX_orhto=x_robot_in_orthonormal_x(targetX,targetY);
-    targetY_orhto=y_robot_in_orthonormal_y(targetX,targetY);
+    set_target_to(HEIGHT_ARENA-40,WIDTH_ARENA-40);//50 cm up and 80 on the right 
 }
 
 //fill initialLogValues with the values we already know (here the bordurs)
@@ -186,12 +199,7 @@
     float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
     float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
     
-    //TODO comment that
-    //pc.printf("\n\r targ_X=%f", target_x);
-    //pc.printf("\n\r targ_Y=%f", target_y);
-    //pc.printf("\n\r targ_Angle=%f", target_angle);
-    
-    go_to_point_with_angle(target_x, target_y, target_angle);
+    go_to_point_with_angle(targetX, targetY, target_angle);
 }
 
 
@@ -213,10 +221,11 @@
 //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(X, Y, target_x, target_y);
+    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;
@@ -260,14 +269,6 @@
             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)
             
-            //Normalize speed for motors
-            if(angularLeft>angularRight) {
-                angularRight=MAX_SPEED*angularRight/angularLeft;
-                angularLeft=MAX_SPEED;
-            } else {
-                angularLeft=MAX_SPEED*angularLeft/angularRight;
-                angularRight=MAX_SPEED;
-            }
             //pc.printf("\n\r X=%f", X);
             //pc.printf("\n\r Y=%f", Y);
     
@@ -275,8 +276,8 @@
             //pc.printf("\n\r a_l=%f", angularLeft);
     
             //Updating motor velocities
-            leftMotor(1,angularLeft);
-            rightMotor(1,angularRight);
+            leftMotor(sign2(angularLeft),abs(angularLeft));
+            rightMotor(sign2(angularRight),abs(angularRight));
     
             wait(0.2);
             //Timer stuff
@@ -441,7 +442,7 @@
             if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
                 pc.printf(" R ");
             else{
-                if(targetY_orhto >= (heightIndiceInOrthonormal+heightMalus) && targetY_orhto <= (heightIndiceInOrthonormal+heightBonus) && targetX_orhto >= (widthIndiceInOrthonormal+widthMalus) && targetX_orhto <= (widthIndiceInOrthonormal+widthBonus))                    
+                if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus))                    
                     pc.printf(" T ");
                 else{
                     currProba=log_to_proba(map[x][y]);
@@ -572,7 +573,7 @@
 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(X, Y, target_x, target_y);
+    *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto);
     *d2 = *distanceFromTarget;
     *beta = -*angleError-theta+target_angle;        
     
@@ -598,18 +599,31 @@
         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 updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
+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 frame
     float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
     float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
     //compute the distance beetween the cell and the robot
     float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
     //check if the cell is in range
-    if(distanceCellToRobot <= (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);
@@ -629,16 +643,16 @@
      //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++){
-            updateForce(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
+            update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
         }
     }
     //update with attraction force
     *forceX=-forceRepulsionComputedX;
     *forceY=-forceRepulsionComputedY;
-    float distanceTargetRobot=sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2));
+    float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2));
     if(distanceTargetRobot != 0){
-        *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/distanceTargetRobot;
-        *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/distanceTargetRobot;
+        *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
@@ -650,13 +664,37 @@
 //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){
     *line_a=forceY;
-    *line_b=forceX;
+    *line_b=-forceX;
     //TODO check in what referentiel it needs to go
     float xRobotOrtho=robot_center_x_in_orthonormal_x();
     float yRobotOrtho=robot_center_y_in_orthonormal_y();
     *line_c=forceX*yRobotOrtho-forceY*xRobotOrtho;    
 }
 
+void test_got_to_line(bool* reached){
+    float line_a=1;
+    float line_b=2;
+    float line_c=-140;
+    //we update the odometrie
+    Odometria();
+    float angularRight=0;
+    float angularLeft=0;
+
+    go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
+    pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
+
+    leftMotor(sign2(angularLeft),abs(angularLeft));
+    rightMotor(sign2(angularRight),abs(angularRight));
+    
+    pc.printf("\r\n angR=%f", angularRight);
+    pc.printf("\r\n angL=%f", angularLeft);
+    pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto));
+
+    //wait(0.1);
+    Odometria();
+    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
+        *reached=true;
+}
 void vff(bool* reached){
     float line_a;
     float line_b;
@@ -679,30 +717,23 @@
     //we compute a new ine
     calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c);
     go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
-     //Normalize speed for motors
-    if(angularLeft>angularRight) {
-        angularRight=MAX_SPEED*angularRight/angularLeft;
-        angularLeft=MAX_SPEED;
-    }
-    else {
-        angularLeft=MAX_SPEED*angularLeft/angularRight;
-        angularRight=MAX_SPEED;
-    }
+     
     pc.printf("\r\n forceX=%f", forceX);
     pc.printf("\r\n forceY=%f", forceY);
     pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
 
     //Updating motor velocities
-    leftMotor(1,angularLeft);
-    rightMotor(1,angularRight);
+    
+    leftMotor(sign2(angularLeft),abs(angularLeft));
+    rightMotor(sign2(angularRight),abs(angularRight));
     
     pc.printf("\r\n angR=%f", angularRight);
     pc.printf("\r\n angL=%f", angularLeft);
-    pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY));
+    pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto));
 
     //wait(0.1);
     Odometria();
-    if(dist(X,Y,targetX,targetY)<10)
+    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
         *reached=true;
 }
 
@@ -717,7 +748,7 @@
     else{
         lineAngle=1.5708;
     }
-
+    
     //Computing angle error
     angleError = lineAngle-theta;
     angleError = atan(sin(angleError)/cos(angleError));
@@ -725,6 +756,42 @@
     //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;
+    //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 theta=%f", theta);
+    pc.printf("\r\n angleError=%f", angleError);
+    pc.printf("\r\n X=%f", robot_center_x_in_orthonormal_x());
+    pc.printf("\r\n Y=%f", robot_center_y_in_orthonormal_y());
+    pc.printf("\r\n TX=%f", targetXOrhto);
+    pc.printf("\r\n TY=%f", targetYOrhto);    
+}
+
+//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;
 }
\ No newline at end of file