Diff: main.cpp

Revision:

39:dd8326ec75ce

Parent:

38:3c9f8cbf5250

Child:

40:f5e212d9f900

--- a/main.cpp	Thu May 18 13:44:31 2017 +0000
+++ b/main.cpp	Thu May 18 15:06:25 2017 +0000
@@ -4,6 +4,7 @@
 
 using namespace std;
 
+void initialise_parameters();
 //fill initialLogValues with the values we already know (here the bordurs)
 void fill_initial_log_values();
 //generate a position randomly and makes the robot go there while updating the map
@@ -20,6 +21,8 @@
 void print_final_map();
 //print the map with the robot marked on it
 void print_final_map_with_robot_position();
+//print the map with the robot and the target marked on it
+void print_final_map_with_robot_position_and_target();
 //go to a given line
 void go_to_line();
 //calculate virtual force field and move
@@ -37,6 +40,8 @@
 float rad_angle_check(float 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);
+float x_robot_in_orthonormal_x(float x, float y);
+float y_robot_in_orthonormal_y(float x, float y);
 float robot_center_x_in_orthonormal_x();
 float robot_center_y_in_orthonormal_y();
 float robot_sonar_front_x_in_orthonormal_x();
@@ -52,7 +57,7 @@
 //update foceX and forceY if necessary
 void updateForce(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 targetX, float targetY,float* forceX, float* forceY);
+void compute_forceX_and_forceY(float* forceX, float* forceY);
 
 
 const float pi=3.14159;
@@ -110,12 +115,14 @@
 const float RADIUS_WHEELS=3.25;
 const float DISTANCE_WHEELS=7.2;
 
-const int MAX_SPEED=100;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
+const int MAX_SPEED=200;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
+
+//TODO all those global variables are making me sad
 
 float alpha; //angle error
 float rho; //distance from target
 float beta;
-float kRho=12, ka=30, kb=-13, kv=200, kh=200; //Kappa values //TODO check kv, kh for go_to_line
+float kRho=12, ka=30, kb=-13, kv=200, kh=200; //Kappa values
 float linear, angular, angular_left, angular_right;
 float dt=0.5;
 float temp;
@@ -132,38 +139,49 @@
 float line_b;
 float line_c;
 
-float targetX=HEIGHT_ARENA-10;//this is in the robot frame top left
-float targetY=WIDTH_ARENA-10;//this is in the robot frame top left
+//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*2/3;//this is in the robot frame top left
+const float targetY=WIDTH_ARENA*2/3;//this is in the robot frame top left
+float targetX_orhto=0;
+float targetY_orhto=0;
 
 bool reached=false;
 
 int main(){
     
+    initialise_parameters();
+
+   //try to reach the target      
+    while (!reached) {
+        vff(); 
+        print_final_map_with_robot_position_and_target();
+    }
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+    //print the map
+    while(1){
+         print_final_map_with_robot_position_and_target();
+    }
+    
+}
+
+void initialise_parameters(){
     i2c1.frequency(100000);
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
 
     measure_always_on();//TODO check if needed
     wait(0.5);
-    
+    //fill the map with the initial log values
     fill_initial_log_values();
 
     theta=DEFAULT_THETA;
     X=DEFAULT_X;
     Y=DEFAULT_Y;
-        
-    while (!reached) {
-        vff(); 
-        print_final_map_with_robot_position();
-    }
-    
-    //Stop at the end
-    leftMotor(1,0);
-    rightMotor(1,0);
-    
-    while(1){
-         print_final_map_with_robot_position();
-    }
+    targetX_orhto=x_robot_in_orthonormal_x(targetX,targetY);
+    targetY_orhto=y_robot_in_orthonormal_y(targetX,targetY);
 }
 
 //fill initialLogValues with the values we already know (here the bordurs)
@@ -179,6 +197,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
@@ -402,6 +422,48 @@
     }
 }
 
+void print_final_map_with_robot_position_and_target() {
+    float currProba;
+    Odometria();
+    float Xrobot=robot_center_x_in_orthonormal_x();
+    float Yrobot=robot_center_y_in_orthonormal_y();
+    
+    float heightIndiceInOrthonormal;
+    float widthIndiceInOrthonormal;
+    
+    float widthMalus=-(3*sizeCellWidth/2);
+    float widthBonus=sizeCellWidth/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(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))                    
+                    pc.printf(" T ");
+                else{
+                    currProba=log_to_proba(map[x][y]);
+                    if ( currProba < 0.5)
+                        pc.printf("   ");
+                    else{
+                        if(currProba==0.5)
+                            pc.printf(" . ");
+                        else
+                            pc.printf(" X ");
+                    } 
+                }
+            }
+        }
+        pc.printf("\n\r");
+    }
+}
+
 //MATHS heavy functions
 /**********************************************************************/
 //Distance computation function
@@ -573,13 +635,11 @@
 }
 
 //compute the force on X and Y
-void compute_forceX_and_forceY(float targetX, float targetY, float* forceX, float* forceY){
+void compute_forceX_and_forceY(float* forceX, float* forceY){
      //we put the position of the robot in an orthonormal frame
      float xRobotOrtho=robot_center_x_in_orthonormal_x();
      float yRobotOrtho=robot_center_y_in_orthonormal_y();
-     //we put the target of the robot in an orthonormal frame
-     float targetXOrtho=x_robot_in_orthonormal_x(targetX,targetY);
-     float targetYOrtho=y_robot_in_orthonormal_y(targetX,targetY);
+
      float forceRepulsionComputedX=0;
      float forceRepulsionComputedY=0;
      //for each cell of the map we compute a force of repulsion
@@ -591,8 +651,8 @@
     //update with attraction force
     *forceX=-forceRepulsionComputedX;
     *forceY=-forceRepulsionComputedY;
-    *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrtho-xRobotOrtho)/sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2));
-    *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrtho-yRobotOrtho)/sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2));
+    *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2));
+    *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2));
     float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
     if(amplitude!=0){//avoid division by 0
         *forceX=*forceX/amplitude;
@@ -600,6 +660,7 @@
     }
 }
 
+//robotX and robotY are from Odometria
 void calculate_line(float forceX, float forceY, float robotX, float robotY){
     line_a=forceY;
     line_b=-forceX;
@@ -619,7 +680,7 @@
     //update the probabilities values 
     update_sonar_values(leftMm, frontMm, rightMm);
     //we compute the force on X and Y
-    compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY);
+    compute_forceX_and_forceY(&forceX, &forceY);
     //we compute a new ine
     calculate_line(forceX, forceY, X, Y);
     go_to_line();