Diff: main.cpp

Revision:

42:ab25bffdc32b

Parent:

41:39157b310975

Child:

43:ffd5a6d4dd48

--- a/main.cpp	Mon May 22 11:44:38 2017 +0000
+++ b/main.cpp	Mon May 22 13:25:20 2017 +0000
@@ -23,10 +23,10 @@
 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();
+//go to a given line by updating angularLeft and angularRight
+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();
+void vff(bool* reached);
 
 //MATHS heavy functions
 float dist(float robot_x, float robot_y, float target_x, float target_y);
@@ -52,13 +52,16 @@
 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);
-void compute_angles_and_distance(float target_x, float target_y, float target_angle);
-void compute_linear_angular_velocities();
+//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 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* 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);
 
 const float pi=3.14159;
 
@@ -118,22 +121,7 @@
 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
-float linear, angular, angular_left, angular_right;
-float dt=0.5;
-float temp;
-float d2;
-Timer t;
-
-int speed=MAX_SPEED; // Max speed at beggining of movement
-
-float line_a;
-float line_b;
-float line_c;
+const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values
 
 //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
@@ -141,14 +129,13 @@
 float targetX_orhto=0;
 float targetY_orhto=0;
 
-bool reached=false;
-
 int main(){
     initialise_parameters();
     
-   //try to reach the target      
+   //try to reach the target     
+   bool reached=false; 
     while (!reached) {
-        vff(); 
+        vff(&reached); 
         print_final_map_with_robot_position_and_target();
     }
     //Stop at the end
@@ -192,8 +179,6 @@
     }
 }
 
-
-
 //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
@@ -229,15 +214,20 @@
 //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) {
     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;
+    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 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();
@@ -267,18 +257,26 @@
         }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)
-    
+            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);
     
-            //pc.printf("\n\r a_r=%f", angular_right);
-            //pc.printf("\n\r a_l=%f", angular_left);
+            //pc.printf("\n\r a_r=%f", angularRight);
+            //pc.printf("\n\r a_l=%f", angularLeft);
     
             //Updating motor velocities
-            leftMotor(1,angular_left);
-            rightMotor(1,angular_right);
+            leftMotor(1,angularLeft);
+            rightMotor(1,angularRight);
     
             wait(0.2);
             //Timer stuff
@@ -320,14 +318,14 @@
 //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){
 
-    float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
-    float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition;
-    alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
+    float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
+    float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
+    anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
     float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
 
     //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
     //check if absolute difference between the angles is no more than Omega/2
-    if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
+    if( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2))){
         if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
         //point before obstacle, probably empty
         /*****************************************************************************/
@@ -570,48 +568,38 @@
     return sizeCellHeight/2+j*sizeCellHeight;
 }
 
-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;        
+//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(X, Y, target_x, target_y);
+    *d2 = *distanceFromTarget;
+    *beta = -*angleError-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));
+    *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);
 }
 
-void compute_linear_angular_velocities(){
+//update angularLeft and angularRight
+void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta,float* angularLeft, float* angularRight){
     //Computing linear and angular velocities
-    if(alpha>=-1.5708 && alpha<=1.5708){
-        linear=kRho*rho;
-        angular=ka*alpha+kb*beta;
+    float linear;
+    float angular;
+    if(angleError>=-1.5708 && angleError<=1.5708){
+        linear=KRHO*distanceFromTarget;
+        angular=KA*angleError+KB*beta;
     }
     else{
-        linear=-kRho*rho;
-        angular=-ka*alpha-kb*beta;
+        linear=-KRHO*distanceFromTarget;
+        angular=-KA*angleError-KB*beta;
     }
-    angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-    angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-    
-    //Slowing down at the end for more precision
-    // if (d2<25) {
-    //     speed = d2*30;
-    // }
-    
-    //Normalize speed for motors
-    if(angular_left>angular_right) {
-        angular_right=speed*angular_right/angular_left;
-        angular_left=speed;
-    } else {
-        angular_left=speed*angular_left/angular_right;
-        angular_right=speed;
-    }
+    *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+    *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
 }
 
 void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
@@ -659,17 +647,20 @@
     }
 }
 
-//robotX and robotY are from Odometria
-void calculate_line(float forceX, float forceY, float robotX, float robotY){
-    line_a=forceY;
-    line_b=forceX;
+//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;    
+    *line_c=forceX*yRobotOrtho-forceY*xRobotOrtho;    
 }
 
-void vff(){
+void vff(bool* reached){
+    float line_a;
+    float line_b;
+    float line_c;
     //Updating X,Y and theta with the odometry values
     float forceX=0;
     float forceY=0;
@@ -679,57 +670,61 @@
     float leftMm = get_distance_left_sensor();
     float frontMm = get_distance_front_sensor();
     float rightMm = get_distance_right_sensor();
+    float angularRight=0;
+    float angularLeft=0;
     //update the probabilities values 
     update_sonar_values(leftMm, frontMm, rightMm);
     //we compute the force on X and Y
     compute_forceX_and_forceY(&forceX, &forceY);
     //we compute a new ine
-    calculate_line(forceX, forceY, X, Y);
-    go_to_line();
+    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,angular_left);
-    rightMotor(1,angular_right);
+    leftMotor(1,angularLeft);
+    rightMotor(1,angularRight);
     
-    pc.printf("\r\n angR=%f", angular_right);
-    pc.printf("\r\n angL=%f", angular_left);
+    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));
 
     //wait(0.1);
     Odometria();
     if(dist(X,Y,targetX,targetY)<10)
-        reached=true;
+        *reached=true;
 }
 
-void go_to_line(){
-    float line_angle, angle_error;
+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;
     if(line_b!=0){
-        line_angle=atan(-line_a/line_b);
+        lineAngle=atan(-line_a/line_b);
     }
     else{
-        line_angle=1.5708;
+        lineAngle=1.5708;
     }
 
     //Computing angle error
-    angle_error = line_angle-theta;
-    angle_error = atan(sin(angle_error)/cos(angle_error));
+    angleError = lineAngle-theta;
+    angleError = atan(sin(angleError)/cos(angleError));
 
     //Calculating velocities
-    linear=kv*(3.1416);
-    angular=kh*angle_error;
-    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=speed*angular_right/angular_left;
-        angular_left=speed;
-    }
-    else {
-        angular_left=speed*angular_left/angular_right;
-        angular_right=speed;
-    }
+    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;
 }
\ No newline at end of file