Diff: main.cpp

Revision:

23:901fc468b8a7

Parent:

22:ebb37a249b5f

Child:

24:8f4b820d8de8

--- a/main.cpp	Tue Apr 18 17:48:26 2017 +0000
+++ b/main.cpp	Wed Apr 19 10:45:09 2017 +0000
@@ -16,6 +16,11 @@
 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
 //print the map
 void print_final_map();
+//compute the angles and distance used for the velocities
+void compute_angles_and_distance(float target_x, float target_y, float target_angle);
+//compute the linear and 2 angular velocities
+void compute_linear_angular_velocities();
+
 
 //MATHS heavy functions
 float dist(float robot_x, float robot_y, float target_x, float target_y);
@@ -133,7 +138,7 @@
 //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) {
-    bool tooClose=false;
+    bool tooClose=false; //binary variable to know if an obstacle is <10cm away
      do {
         pc.printf("\n\n\r entered while");
         
@@ -145,53 +150,19 @@
         //Updating X,Y and theta with the odometry values
         Odometria();
         
+        //Updare the values from the sonar
         update_sonar_values();
 
+        //Check if one of the sonars finds an obstacle that is too close
         if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
             tooClose = true;
             break;    
         }
         
-        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;        
-        
-        //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));
-        pc.printf("\n\r d2=%f", d2);
-        pc.printf("\n\r dt=%f", dt);
+        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)
 
-        //Computing linear and angular velocities
-        if(alpha>=-1.5708 && alpha<=1.5708){
-            linear=kRho*rho;
-            angular=ka*alpha+kb*beta;
-        }
-        else{
-            linear=-kRho*rho;
-            angular=-ka*alpha-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;
-        }
-
+        //Printing values for debugging purposes
         pc.printf("\n\r X=%f", X);
         pc.printf("\n\r Y=%f", Y);
         pc.printf("\n\r leftMm=%f", leftMm);
@@ -353,3 +324,46 @@
         return acos(cosAlpha);
 }
 
+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;        
+    
+    //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));
+    pc.printf("\n\r d2=%f", d2);
+    pc.printf("\n\r dt=%f", dt);
+}
+
+void compute_linear_angular_velocities(){
+    //Computing linear and angular velocities
+    if(alpha>=-1.5708 && alpha<=1.5708){
+        linear=kRho*rho;
+        angular=ka*alpha+kb*beta;
+    }
+    else{
+        linear=-kRho*rho;
+        angular=-ka*alpha-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;
+    }
+}