Diff: main.cpp

Revision:

3:1e0f4cb93eda

Parent:

2:ea61e801e81f

Child:

4:8c56c3ba6e54

--- a/main.cpp	Tue Mar 21 16:17:56 2017 +0000
+++ b/main.cpp	Fri Mar 24 15:46:46 2017 +0000
@@ -9,12 +9,16 @@
     pc.baud(9600); // baud for the pc communication
 
     //Target example x,y values
-    float target_x=46.8, target_y=78.6;
+    float target_x=46.8, target_y=78.6, target_angle=0;
 
-    float angle_error; //angle error
+    float a; //angle error
     float d; //distance from target
-    float k_linear=10, k_angular=200;
+    float beta;
+    //float k_linear=10, k_angular=200;
+    float kr=3, ka=8, kb=-2;
     float linear, angular, angular_left, angular_right;
+    float dt=0.5;
+    float temp;
 
     //Diameter of a wheel and distance between the 2
     float r=3.25, b=7.2;
@@ -26,21 +30,38 @@
     X=0;
     Y=0;
 
+    a = atan2((target_y-Y),(target_x-X))-theta;
+    a = atan(sin(a)/cos(a));
+    d = dist(X, Y, target_x, target_y);
+    beta = -a-theta+target_angle;
+    
     do {
         pc.printf("\n\n\r entered while");
         
         //Updating X,Y and theta with the odometry values
         Odometria();
+        
+        a = atan2((target_y-Y),(target_x-X))-theta;
+        a = atan(sin(a)/cos(a));
+        d = dist(X, Y, target_x, target_y);
+        beta = -a-theta+target_angle;
 
         //Computing angle error and distance towards the target value
-        angle_error = atan2((target_y-Y),(target_x-X))-theta;
-        angle_error = atan(sin(angle_error)/cos(angle_error));
-        d=dist(X, Y, target_x, target_y);
+        d += dt*(-kr*cos(a)*d);
+        a = temp;
+        a += dt*(kr*sin(a)-ka*a-kb*b);
+        b += dt*(-kr*sin(temp));
         pc.printf("\n\r d=%f", d);
 
         //Computing linear and angular velocities
-        linear=k_linear*d;
-        angular=k_angular*angle_error;
+        if(a>=-1.5708 && a<=1.5708){
+            linear=kr*d;
+            angular=ka*a+kb*beta;
+        }
+        else{
+            linear=-kr*d;
+            angular=-ka*a-kb*beta;
+        }
         angular_left=(linear-0.5*b*angular)/r;
         angular_right=(linear+0.5*b*angular)/r;
 
@@ -65,7 +86,7 @@
         leftMotor(1,angular_left);
         rightMotor(1,angular_right);
 
-        wait(0.5);
+        wait(dt);
     } while(d>1);
 
     //Stop at the end