Diff: main.cpp

Revision:

9:699054d8510b

Parent:

8:ed59eb8437c4

Child:

10:32c65de8ff37

--- a/main.cpp	Fri Apr 05 16:26:58 2019 +0000
+++ b/main.cpp	Mon Apr 08 21:25:14 2019 +0000
@@ -3,7 +3,7 @@
 #include "math.h"
 #include "ActiveCell.h"
 #include "HistogramCell.h"
-#define M_PI 3.14159265358979323846
+#define M_PI 3.14159265358979323846f
 //EXERCICIO 1
 //Luis Cruz N2011164454
 //Jacek Sobecki N2018319609
@@ -44,15 +44,16 @@
     //Cells dim: 5x5cm |
     float  w[3], v, theta, theta_error, err, integral = 0.0, k_i = 0.01/*0.02*/;
     const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/, 
-    k_s = 12/*10*/, sample_time = 0.05, d_stalker = 7.5, k_f = 2.5; /*2.5*/ //VFF
+    k_s = 12/*10*/, sample_time = 0.05, d_stalker = 5, k_f = 12.5; /*7.5*/ //VFF
+    float theta_final;
 // ===============================================================================
 // =================================== COORDS ====================================
 // =============================================================================== 
     //Target coordinates
-    p_final[0] = 150, p_final[1] = 200, p_final[2] = 0;
+    p_final[0] = 200, p_final[1] = 20, p_final[2] = 0;
     //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
     //Initial coordinates:
-    p[0] = 95, p[1] = 20, p[2] = 0;
+    p[0] = 20, p[1] = 20, p[2] = 0;
 // ===============================================================================
 // =================================== EXECUTION =================================
 // ===============================================================================
@@ -65,20 +66,30 @@
         
         //Path calculation
         poseEst(p, radius, enc_res, b); //Pose estimation
-        updateActive(p[0], p[1],theta);
+        theta_final = atan2(p_final[1]-p[1],p_final[0]-p[0]);
+        theta_final = atan2(sin(theta_final),cos(theta_final));
+        updateActive(p[0], p[1], theta_final);
         p_obj[0] = p[0]+k_f*fX; // add parameter to relate chosen direction (VFH) to the point nearby of the robot
         p_obj[1] = p[1]+k_f*fY;
         //Control Law
         err = sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2)) - d_stalker; //distance to the point
         theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
+        pc.printf("theta MAIN: = %lf\n\n", theta);
         theta = atan2(sin(theta),cos(theta));
+        
+
+        
         p[2] = atan2(sin(p[2]),cos(p[2]));
         theta_error = theta-p[2];
+        theta_error = atan2(sin(theta_error),cos(theta_error));
+        pc.printf("theta_error = %lf | p[2]= %lf\n\n", theta_error, p[2]);
         w[0] = k_s*(theta_error); //direction gain
         integral += err;
         v = k_v*err+k_i*integral; //Speed calculation
         w[1] = (v-(b/2)*w[0])/radius;
         w[2] = (v+(b/2)*w[0])/radius;
+        
+pc.printf("w0 = %lf | w1 = %lf | w2 = %lf\n",  w[0], w[1], w[2]);
         SpeedLim(w);
         //if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 70) k_i = -0.005;
         if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 4){
@@ -96,9 +107,9 @@
 //Pose Estimation function
 void poseEst(float p[], float radius, float enc_res, float b){
     float deltaDl, deltaDr, deltaD, deltaT;
-    deltaDl = ((float)countsLeft)*(2.0*M_PI*radius/enc_res);
-    deltaDr = ((float)countsRight)*(2.0*M_PI*radius/enc_res);
-    deltaD = (deltaDr + deltaDl)/2;
+    deltaDl = ((float)countsLeft)*(2.0f*M_PI*radius/enc_res);
+    deltaDr = ((float)countsRight)*(2.0f*M_PI*radius/enc_res);
+    deltaD = (deltaDr + deltaDl)/2.0f;
     deltaT = (deltaDr - deltaDl)/b;
     if(fabs(deltaT) == 0){
         p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
@@ -147,8 +158,8 @@
     for (int i = 0; i < hSize; i++) {
         for (int j = 0; j < hSize; j++) {
             histogram[i][j].calculate(i, j);
-            if(i>15 && i<25 && j > 15 && j<25)
-                histogram[i][j].cellVal=5;
+            /*if(i>16 && i<26 && j > 0 && j<8)
+                histogram[i][j].cellVal=5;*/
         }
     }
     for (int i = 0; i < aSize; i++) {
@@ -221,13 +232,16 @@
         else
             temp[i]=0;
     }
-    int best=0;
-    theta=theta*180/3.14159265358979323846;
-    for (int i = 1; i < 30; ++i) {
-        if(temp[i]!=0 && fabs(12*i-theta)<fabs(12*best-theta))
-            best=i;
+    float best=999;
+    float theta_deg;
+    theta_deg =theta*180.0f/3.14159265358979323846f;
+    if(theta_deg<0) theta_deg=360.0f+theta_deg;
+    pc.printf("theta corrr: = %lf\n\n", theta_deg);
+    for (int i = 0; i < 30; ++i) {
+        if(temp[i]!=0 && fabs(12*i-theta_deg)<fabs(12*best-theta_deg)) best=i;
     }
-    fX=cos(best*12*M_PI/180);
-    fY=sin(best*12*M_PI/180);
+    pc.printf("best = %lf\n\n", best);
+    fX=cos(best*12.0f*M_PI/180.0f);
+    fY=sin(best*12.0f*M_PI/180.0f);
     
 }
\ No newline at end of file