Diff: main.cpp

Revision:

4:8c56c3ba6e54

Parent:

3:1e0f4cb93eda

Child:

5:dea05b8f30d0

--- a/main.cpp	Fri Mar 24 15:46:46 2017 +0000
+++ b/main.cpp	Fri Mar 24 16:30:30 2017 +0000
@@ -2,8 +2,10 @@
 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
 #include "math.h"
 
+
 float dist(float robot_x, float robot_y, float target_x, float target_y);
 
+//Timeout time;
 int main(){
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
@@ -11,11 +13,11 @@
     //Target example x,y values
     float target_x=46.8, target_y=78.6, target_angle=0;
 
-    float a; //angle error
-    float d; //distance from target
+    float alpha; //angle error
+    float rho; //distance from target
     float beta;
     //float k_linear=10, k_angular=200;
-    float kr=3, ka=8, kb=-2;
+    float kRho=3, ka=8, kb=-2;
     float linear, angular, angular_left, angular_right;
     float dt=0.5;
     float temp;
@@ -30,10 +32,10 @@
     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;
+    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;
     
     do {
         pc.printf("\n\n\r entered while");
@@ -41,33 +43,34 @@
         //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;
+        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;
+        //beta = -alpha-theta+target_angle;
 
         //Computing angle error and distance towards the target value
-        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);
+        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);
 
         //Computing linear and angular velocities
-        if(a>=-1.5708 && a<=1.5708){
-            linear=kr*d;
-            angular=ka*a+kb*beta;
+        if(alpha>=-1.5708 && alpha<=1.5708){
+            linear=kRho*rho;
+            angular=ka*alpha+kb*beta;
         }
         else{
-            linear=-kr*d;
-            angular=-ka*a-kb*beta;
+            linear=-kRho*rho;
+            angular=-ka*alpha-kb*beta;
         }
         angular_left=(linear-0.5*b*angular)/r;
         angular_right=(linear+0.5*b*angular)/r;
 
         //Slowing down at the end for more precision
-        if (d<25) {
-            speed = d*30;
+        if (rho<25) {
+            speed = rho*30;
         }
         
         //Normalize speed for motors
@@ -87,16 +90,23 @@
         rightMotor(1,angular_right);
 
         wait(dt);
-    } while(d>1);
+    } while(rho>1);
 
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
 
-    pc.printf("\n\r %f -- arrived!", d);
+    pc.printf("\n\r %f -- arrived!", rho);
 }
 
 //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));
-}
\ No newline at end of file
+}
+
+/*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