Diff: main.cpp

Revision:

3:a0b0e4acf5e9

Parent:

2:c507076bfd93

--- a/main.cpp	Fri Mar 22 15:22:15 2019 +0000
+++ b/main.cpp	Fri Mar 22 16:00:45 2019 +0000
@@ -10,7 +10,8 @@
 void poseEst(float p[], float radius, float enc_res, float b);
 void SpeedLim(float w[]);
 //int ReadSensors();
-void updateHist(int hist[][], int active[][],int x,int y);
+const int n = 200, activeSize = 11;
+void updateHist(int hist[][n], int active[][activeSize],int x,int y);
 int main(){
 
     button.mode(PullUp);
@@ -24,16 +25,16 @@
     //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
     //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
     //Cells dim: 5x5cm |
-    const int = m = 200, n = 200, activeSize = 11;
+    const int m = 200;
     int hist[m][n] , active[activeSize][activeSize];
     float  w[3], v, p[3], p_obj[3], theta, theta_error, err, integral = 0.0;
     const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 7, 
-        k_s = 60, k_i = 1, sample_time = 0.05, Fcr = 1.0, d_stalker = 5.0;
+        k_s = 60, k_i = 0.0, sample_time = 0.05, Fcr = 1.0, d_stalker = 3.0;
 // ===============================================================================
 // =================================== COORDS ====================================
 // =============================================================================== 
     //Target coordinates
-    p_obj[0] = 400, p_obj[1] = 400, p_obj[2] = 0;
+    p_obj[0] = 200, p_obj[1] = 200, p_obj[2] = 0;
     //Initial coordinates:
     p[0] = 100, p[1] = 100, p[2] = 0;
 // ===============================================================================
@@ -42,8 +43,8 @@
     while(1){
         getCountsAndReset();
         pc.printf("Speeds: Left=%lf   Right=%lf\n", w[1], w[2]);
-        pc.printf("Odometer: X=%lf   Y=%lf   Theta=%lf\n", p[0], p[1], p[2]);
-        pc.printf("Position: X=%lf   Y=%lf   Theta=%lf\n", p[0], p[1], p[2]);
+        //pc.printf("Odometer: X=%lf   Y=%lf   Theta=%lf\n", p[0], p[1], p[2]);
+        pc.printf("Position: X=%lf   Y=%lf   Theta=%lf\n\n", p[0], p[1], p[2]);
         
         //Path calculation
         poseEst(p, radius, enc_res, b); //Pose estimation
@@ -54,12 +55,13 @@
         p[2] = atan2(sin(p[2]),cos(p[2]));
         theta_error = theta-p[2];
         w[0] = k_s*(theta_error); //direction gain
-        integral += err
+        integral += err;
         v = k_v*err+k_i*integral; //Speed gain
         w[1] = (v-(b/2)*w[0])/radius;
         w[2] = (v+(b/2)*w[0])/radius;
         SpeedLim(w);
-        if((fabs(p[0]-p_obj[0])+fabs(p[1]-p_obj[1])) < 1){
+        if((fabs(p[0]-p_obj[0])+fabs(p[1]-p_obj[1])) < 5){
+            pc.printf("\n YOU REACHED YOUR DESTINATION, THANK YOU FOR CHOOSING EMIRATES\n");
             setSpeeds(0,0);
         }
         else{
@@ -105,22 +107,22 @@
             w[1] = 150;
         }
     }
-    if(w[2] < 50 || w[1] < 50){
+    if(w[2] < 35 || w[1] < 35){
         if(wratio < 1){
-            w[1] = 50;
+            w[1] = 35;
             w[2] = w[1]*wratio;
         }
         else if(wratio > 1){
-            w[2] = 50;
+            w[2] = 35;
             w[1] = w[2]/wratio;
         }
         else{
-            w[2] = 50;
-            w[1] = 50;
+            w[2] = 35;
+            w[1] = 35;
         }
     }
 }
-void updateHist(int hist[i][j], int active[activeSize][activeSize],int x,int y){
+void updateHist(int hist[][n], int active[][activeSize],int x,int y){
     int aX=0;
     for(int i=x-(activeSize/2);i<x+(activeSize/2+1);i++){
         int aY=0;