Diff: main.cpp

Revision:

6:afde4b08166b

Parent:

5:dea05b8f30d0

Child:

7:c94070f9af78

--- a/main.cpp	Fri Mar 24 16:38:58 2017 +0000
+++ b/main.cpp	Fri Mar 24 17:39:24 2017 +0000
@@ -1,7 +1,8 @@
 #include "mbed.h"
 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
 #include "math.h"
-
+ 
+Timer t;
 
 float dist(float robot_x, float robot_y, float target_x, float target_y);
 
@@ -17,10 +18,11 @@
     float rho; //distance from target
     float beta;
     //float k_linear=10, k_angular=200;
-    float kRho=1, ka=8, kb=-2;
+    float kRho=12, ka=25, kb=-15;
     float linear, angular, angular_left, angular_right;
     float dt=0.5;
     float temp;
+    float d2;
 
     //Diameter of a wheel and distance between the 2
     float r=3.25, b=7.2;
@@ -35,26 +37,37 @@
     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;
+    //beta = atan(sin(beta)/cos(beta));
     
     do {
         pc.printf("\n\n\r entered while");
         
+        //Timer stuff
+        dt = t.read();
+        t.reset();
+        t.start();
+        
         //Updating X,Y and theta with the odometry values
         Odometria();
         
         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;
         beta = -alpha-theta+target_angle;
-
+        //beta = atan(sin(beta)/cos(beta));
+        
+        
         //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*b);
-        b += dt*(-kRho*sin(temp));
-        pc.printf("\n\r rho=%f", rho);
+        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){
@@ -69,8 +82,8 @@
         angular_right=(linear+0.5*b*angular)/r;
 
         //Slowing down at the end for more precision
-        if (rho<25) {
-            speed = rho*30;
+        if (d2<25) {
+            speed = d2*30;
         }
         
         //Normalize speed for motors
@@ -89,8 +102,10 @@
         leftMotor(1,angular_left);
         rightMotor(1,angular_right);
 
-        wait(dt);
-    } while(rho>1);
+        wait(0.5);
+        //Timer stuff
+        t.stop();
+    } while(d2>2);
 
     //Stop at the end
     leftMotor(1,0);
@@ -102,11 +117,4 @@
 //Distance computation function
 float dist(float robot_x, float robot_y, float target_x, float target_y){
     return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
-}
-
-/*static double diffclock(clock_t clock1,clock_t clock2)
-{
-    double diffticks=clock1-clock2;
-    double diffms=(diffticks)/(CLOCKS_PER_SEC/1000);
-    return diffms;
-}*/
\ No newline at end of file
+}
\ No newline at end of file