Diff: main.cpp

Revision:

44:e2bd06f94dc0

Parent:

43:ffd5a6d4dd48

Child:

45:fb07065a64a9

--- a/main.cpp	Mon May 22 18:20:08 2017 +0000
+++ b/main.cpp	Mon May 29 11:52:56 2017 +0000
@@ -68,17 +68,18 @@
 //return 1 if positiv, 0 if negativ
 int sign2(float value);
 void set_target_to(float x, float y);
+void try_to_reach_target();
 
 const float pi=3.14159;
 
 //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
+//TODO MEASURE THE DISTANCE on X and Y of the robot space, between each sonar and the center of the robot and add it to calculus in updateSonarValues
 const float RANGE_SONAR=50;//cm
 const float RANGE_SONAR_MIN=10;//Rmin cm
 const float INCERTITUDE_SONAR=10;//cm
 const float ANGLE_SONAR=pi/3;//Omega rad
 
-//those distance and angle are approximation in need of measurements, in the orthonormal frame
+//those distance and angle are approximation in need of measurements, in the orthonormal space
 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
 const float DISTANCE_SONAR_LEFT_X=-4;
 const float DISTANCE_SONAR_LEFT_Y=4;
@@ -99,7 +100,7 @@
 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 frame
+//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;
@@ -130,30 +131,53 @@
 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)
-float targetX;//this is in the robot frame top left
-float targetY;//this is in the robot frame top left
+float targetX;//this is in the robot space top left
+float targetY;//this is in the robot space top left
 float targetXOrhto;
 float targetYOrhto;
 
 int main(){
-    initialise_parameters();
-    
-   //try to reach the target     
-   bool reached=false; 
+    initialise_parameters();   
+    //try to reach the target     
+    set_target_to(50,0);//up right
+    print_final_map_with_robot_position_and_target();
+    try_to_reach_target();
+    set_target_to(-50,0);//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();
+    //print the map forever
+    while(1){
+         print_final_map_with_robot_position_and_target();
+    }
+}
+
+void try_to_reach_target(){
+    bool reached=false;
+    int print=0;
     while (!reached) {
         vff(&reached);
         //test_got_to_line(&reached);
-        print_final_map_with_robot_position_and_target();
+        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
+            print+=1;
     }
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
-    
-    //print the map forever
-    while(1){
-         print_final_map_with_robot_position_and_target();
-    }
-    
+    pc.printf("\r\n target reached");
 }
 
 void set_target_to(float x, float y){
@@ -176,7 +200,6 @@
     theta=DEFAULT_THETA;
     X=DEFAULT_X;
     Y=DEFAULT_Y;
-    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)
@@ -297,7 +320,7 @@
     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 frame is within the range of the sonar (which has the coordinates xs, ys in the world frame)
+                //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);
@@ -517,7 +540,7 @@
 /*
 
 
-Robot frame:      orthonormal frame:
+Robot space:      orthonormal space:
       ^                 ^
       |x                |y
    <- R                 O ->
@@ -617,7 +640,7 @@
 }
 
 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
+    //get the coordonate of the map and the robot in the ortonormal space
     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
@@ -634,7 +657,7 @@
 
 //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 frame
+     //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();
 
@@ -661,16 +684,6 @@
     }
 }
 
-//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;
-    //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;
@@ -686,8 +699,6 @@
     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);
@@ -717,19 +728,11 @@
     //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);
-     
-    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(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();
@@ -737,6 +740,23 @@
         *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;
@@ -757,8 +777,8 @@
     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;
+    *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+    *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
     
     float aL=*angularLeft;
     float aR=*angularRight;
@@ -771,27 +791,33 @@
         *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);    
+    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());
 }
 
-//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;
+//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){
+    /*
+    in the teachers maths it is 
+    
+    *line_a=forceY;
+    *line_b=-forceX;
+    
+    because a*x+b*y+c=0
+    a impact the vertical and b the horizontal
+    and he has to put them like this because
+    Robot space:      orthonormal space:
+      ^                 ^
+      |x                |y
+   <- R                 O ->
+    y                     x
+    but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to 
+    */
+    *line_a=forceX;
+    *line_b=forceY;
+    //TODO check that
+    //because the line computed always pass by the robot center we dont need lince_c
+    //float xRobotOrtho=robot_center_x_in_orthonormal_x();
+    //float yRobotOrtho=robot_center_y_in_orthonormal_y();
+    //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho;    
+    *line_c=0;
 }
\ No newline at end of file