Diff: main.cpp

Revision:

40:f5e212d9f900

Parent:

39:dd8326ec75ce

Child:

41:39157b310975

--- a/main.cpp	Thu May 18 15:06:25 2017 +0000
+++ b/main.cpp	Thu May 18 18:48:47 2017 +0000
@@ -99,8 +99,8 @@
 //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=10;//lower right
-const float DEFAULT_Y=10;//lower right
+const float DEFAULT_X=20;//lower right
+const float DEFAULT_Y=20;//lower right
 const float DEFAULT_THETA=0;
 
 //used to create the map 250 represent the 250cm of the square where the robot is tested
@@ -131,36 +131,31 @@
 
 int speed=MAX_SPEED; // Max speed at beggining of movement
 
-float leftMm;
-float frontMm;
-float rightMm;
-
 float line_a;
 float line_b;
 float line_c;
 
 //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
+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;
 
 bool reached=false;
 
 int main(){
+    initialise_parameters();
     
-    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
+    
+    //print the map forever
     while(1){
          print_final_map_with_robot_position_and_target();
     }
@@ -240,6 +235,9 @@
     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();
@@ -616,9 +614,7 @@
     }
 }
 
-
 void updateForce(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);
@@ -651,10 +647,13 @@
     //update with attraction force
     *forceX=-forceRepulsionComputedX;
     *forceY=-forceRepulsionComputedY;
-    *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 distanceTargetRobot=sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2));
+    if(distanceTargetRobot != 0){
+        *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/distanceTargetRobot;
+        *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/distanceTargetRobot;
+    }
     float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
-    if(amplitude!=0){//avoid division by 0
+    if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
         *forceX=*forceX/amplitude;
         *forceY=*forceY/amplitude;
     }
@@ -663,8 +662,11 @@
 //robotX and robotY are from Odometria
 void calculate_line(float forceX, float forceY, float robotX, float robotY){
     line_a=forceY;
-    line_b=-forceX;
-    line_c=forceX*robotY-forceY*robotX;    
+    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 vff(){
@@ -674,9 +676,9 @@
     //we update the odometrie
     Odometria();
     //we check the sensors
-    leftMm = get_distance_left_sensor();
-    frontMm = get_distance_front_sensor();
-    rightMm = get_distance_right_sensor();
+    float leftMm = get_distance_left_sensor();
+    float frontMm = get_distance_front_sensor();
+    float rightMm = get_distance_right_sensor();
     //update the probabilities values 
     update_sonar_values(leftMm, frontMm, rightMm);
     //we compute the force on X and Y
@@ -696,8 +698,7 @@
     pc.printf("\r\n angL=%f", angular_left);
     pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY));
 
-
-    wait(0.1);
+    //wait(0.1);
     Odometria();
     if(dist(X,Y,targetX,targetY)<10)
         reached=true;