Diff: main.cpp

Revision:

2:c507076bfd93

Parent:

1:ef1f9f676295

Child:

3:a0b0e4acf5e9

Child:

4:5a892f5ab5a8

--- a/main.cpp	Mon Mar 18 11:29:27 2019 +0000
+++ b/main.cpp	Fri Mar 22 15:22:15 2019 +0000
@@ -9,10 +9,10 @@
 DigitalIn button(PC_13);
 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);
 int main(){
 
-//test
-
     button.mode(PullUp);
     getCountsAndReset();
     setSpeeds(0, 0);
@@ -23,32 +23,39 @@
     //p_obj[0] = X     | p_obj[1] = Y | p_obj[2] = Theta
     //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
-    float  w[3], v, p[3], p_obj[3], theta, theta_error;
-    const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 7, k_s = 60, sample_time = 0.05;
+    //Cells dim: 5x5cm |
+    const int = m = 200, n = 200, activeSize = 11;
+    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;
 // ===============================================================================
 // =================================== COORDS ====================================
-// ===============================================================================
+// =============================================================================== 
     //Target coordinates
-    p_obj[0] = -40, p_obj[1] = 10, p_obj[2] = 0;
+    p_obj[0] = 400, p_obj[1] = 400, p_obj[2] = 0;
     //Initial coordinates:
-    p[0] = 0, p[1] = 0, p[2] = 0;
+    p[0] = 100, p[1] = 100, p[2] = 0;
 // ===============================================================================
 // =================================== EXECUTION =================================
 // ===============================================================================
     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]);
-
+        
         //Path calculation
-        poseEst(p, radius, enc_res, b);
+        poseEst(p, radius, enc_res, b); //Pose estimation
+        //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]);
         theta = atan2(sin(theta),cos(theta));
         p[2] = atan2(sin(p[2]),cos(p[2]));
         theta_error = theta-p[2];
-        w[0] = k_s*(theta_error);
-        //pc.printf("\nOmega:%lf     a:%lf\n", w[0], theta);
-        v = k_v*sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2));
+        w[0] = k_s*(theta_error); //direction gain
+        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);
@@ -113,3 +120,16 @@
         }
     }
 }
+void updateHist(int hist[i][j], int active[activeSize][activeSize],int x,int y){
+    int aX=0;
+    for(int i=x-(activeSize/2);i<x+(activeSize/2+1);i++){
+        int aY=0;
+        for(int j=y-(activeSize/2);j<y+(activeSize/2+1);j++){
+            if(i>=0 && j>=0){
+                hist[i][j]=active[aX][aY];
+                aY++;
+            }
+        }
+        aX++;
+    }
+}
\ No newline at end of file