Diff: main.cpp

Revision:

26:b020cf253059

Parent:

25:572c9e9a8809

Child:

27:07bde633af72

--- a/main.cpp	Thu Apr 20 15:02:28 2017 +0000
+++ b/main.cpp	Thu Apr 20 18:54:48 2017 +0000
@@ -94,6 +94,22 @@
 bool tooClose=false;
 bool turnFromObstacle=false;
 
+float alpha; //angle error
+float rho; //distance from target
+float beta;
+float kRho=12, ka=30, kb=-13; //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 leftMm;
+float frontMm;
+float rightMm; 
+
 int main(){
     
     i2c1.frequency(100000);
@@ -155,23 +171,6 @@
         target_angle = atan(sin(target_angle)/cos(target_angle));
         pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", target_angle);
     }
-
-    //TODO ugly old stuff
-    float alpha; //angle error
-    float rho; //distance from target
-    float beta;
-    float kRho=12, ka=30, kb=-13; //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 leftMm;
-    float frontMm;
-    float rightMm; 
         
     alpha = atan2((target_y-Y),(target_x-X))-theta;
     alpha = atan(sin(alpha)/cos(alpha));
@@ -212,48 +211,8 @@
             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);
-
-        //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;
-        
-        pc.printf("\n\r a_r=%f", angular_right);
-        pc.printf("\n\r a_l=%f", angular_left);
-
-        //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;
-        }
+        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)
 
         //pc.printf("\n\r X=%f", X);
         //pc.printf("\n\r Y=%f", Y);
@@ -499,4 +458,48 @@
 }
 float estimated_height_indice_in_orthonormal_y(int j){
     return sizeCellY/2+j*sizeCellY;
+}
+
+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;
+    }
 }
\ No newline at end of file