Revision 6:2cd6ae395c0f, committed 2019-04-05

Comitter:

shut

Date:

Fri Apr 05 14:28:09 2019 +0000

Parent:

5:1649f59c37de

Commit message:

retwerwer

Changed in this revision

--- a/ActiveCell.h	Thu Mar 28 10:59:55 2019 +0000
+++ b/ActiveCell.h	Fri Apr 05 14:28:09 2019 +0000
@@ -8,11 +8,11 @@
 public:
     //cell dimension = 5x5cm
     int cellVal;
-    double forceX;
-    double forceY;
-    double distance;
+    float forceX;
+    float forceY;
+    float distance;
     int xt, yt;
-    static const double repulsiveForce = 1000;
+    static const float repulsiveForce = 20;
     //robots position
     int x0;
     int y0;
@@ -30,7 +30,7 @@
         forceX = (repulsiveForce * cellVal / pow(distance, 2)) * (xt - x0) / distance;
         forceY = (repulsiveForce * cellVal / pow(distance, 2)) * (yt - y0) / distance;
     }
-
+//Calculating distance from the robot 
     void calDist(int idX, int idY) {
         if (idX < 5) {
             xt = x0 - (5 - idX) * 5;
@@ -46,7 +46,7 @@
         } else {
             yt = y0 + (idY - 5) * 5;
         }
-        distance = sqrt(pow((double)abs(x0 - xt), 2) + pow((double)abs(y0 - yt), 2));
+        distance = sqrt(pow((float)abs(x0 - xt), 2) + pow((float)abs(y0 - yt), 2));
     }
 };
 

--- a/HistogramCell.h	Thu Mar 28 10:59:55 2019 +0000
+++ b/HistogramCell.h	Fri Apr 05 14:28:09 2019 +0000
@@ -5,15 +5,14 @@
 class HistogramCell {
 public:
     int cellVal;
-    double x;
-    double y;
+    float x;
+    float y;
 
     void calculate(int idX, int idY){
         //this is cells position in coordinate system
         // we add 2,5cm to get position of middle point of the cell not its starting point
         x=idX*5+2.5;
         y=idY*5+2.5;
-
     }
 };
 

--- a/main.cpp	Thu Mar 28 10:59:55 2019 +0000
+++ b/main.cpp	Fri Apr 05 14:28:09 2019 +0000
@@ -13,20 +13,17 @@
 void SpeedLim(float w[]);
 void initializeArrays();
 void calcForce();
-void updateActive(double xR, double yR);
+void sumForces();
+void updateActive(float xR, float yR);
 //int ReadSensors();
 //const int m = 200, n = 200, activeSize = 11;
-//histogram size
-const int hSize = 60;
-//active region size
-const int aSize = 11;
+//histogram size | aSize active region size
+const int hSize = 80, aSize = 11;
 ActiveCell activeReg[aSize][aSize];
 HistogramCell histogram[hSize][hSize];
 //Repulsive force sums
-double fX, fY;
-
-float p[3], p_obj[3];
-
+float p[3], p_obj[3], p_final[3], fX, fY;
+const float Fca=6;/*5*/
 int main(){
 
     button.mode(PullUp);
@@ -34,34 +31,38 @@
     setSpeeds(0, 0);
     initializeArrays();
     while(button==1);
-
-    //w[0] = Omega     | w[1] = X     | w[2] = Y
+    //w[0] = Omega     | w[1] = Left  | w[2] = Right
     //p[0] = X         | p[1] = Y     | p[2] = Theta
     //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
     //Cells dim: 5x5cm |
-    float  w[3], v, 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;
+    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
 // ===============================================================================
 // =================================== COORDS ====================================
 // =============================================================================== 
     //Target coordinates
-    p_obj[0] = 400, p_obj[1] = 400, p_obj[2] = 0;
+    p_final[0] = 150, p_final[1] = 200, p_final[2] = 0;
+    //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
     //Initial coordinates:
-    p[0] = 100, p[1] = 100, p[2] = 0;
+    p[0] = 95, p[1] = 20, 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]);
+        pc.printf("OBJECTIVE X: %lf  OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]);
+        pc.printf("Position: X=%lf   Y=%lf   Theta=%lf\n\n", p[0], p[1], p[2]);
+        pc.printf("Force (X): X=%lf   Force(Y)=%lf\n", fX, fY);
         
         //Path calculation
         poseEst(p, radius, enc_res, b); //Pose estimation
+        updateActive(p[0], p[1]);
+        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]);
@@ -70,11 +71,12 @@
         theta_error = theta-p[2];
         w[0] = k_s*(theta_error); //direction gain
         integral += err;
-        v = k_v*err+k_i*integral; //Speed gain
+        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;
         SpeedLim(w);
-        if((fabs(p[0]-p_obj[0])+fabs(p[1]-p_obj[1])) < 1){
+        //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){
             setSpeeds(0,0);
         }
         else{
@@ -105,7 +107,7 @@
 //Speed limiter function
 void SpeedLim(float w[]){
     float wratio;
-    wratio = w[2]/w[1];
+    wratio = fabs(w[2]/w[1]);
     if(w[2] > 150 || w[1] > 150){
         if(wratio < 1){
             w[1] = 150;
@@ -140,6 +142,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;
         }
     }
     for (int i = 0; i < aSize; i++) {
@@ -159,13 +163,13 @@
 }
 //every time robot changes position we need to call this function to update active region and calculate forces
 //xR, yR - robots position in coordinates system
-void updateActive(double xR, double yR) {
+void updateActive(float xR, float yR) {
     int idXr = 0;
     int idYr = 0;
     for (int i = 0; i < hSize; i++) {
         for (int j = 0; j < hSize; j++) {
-            if (xR > histogram[i][j].x - 2.5 && xR < histogram[i][j].x + 2.5 && yR > histogram[i][j].y - 2.5 &&
-                yR < histogram[i][j].y + 2.5) {
+            if (xR > histogram[i][j].x - 2.5f && xR < histogram[i][j].x + 2.5f && yR > histogram[i][j].y - 2.5f &&
+                yR < histogram[i][j].y + 2.5f) {
                 idXr = i;
                 idYr = j;
                 break;
@@ -176,7 +180,7 @@
     for (int k = 0; k < aSize; k++) {
         int n = idYr - aSize / 2;
         for (int l = 0; l < aSize; l++) {
-            if (m > 0 && n > 0 && m < hSize && n < hSize) {
+            if(m >= 0 && n >= 0 && m < hSize && n < hSize) {
                 activeReg[k][l].cellVal = histogram[m][n].cellVal;
             }
             n++;
@@ -184,16 +188,16 @@
         m++;
     }
     calcForce();
+    sumForces();
 }
 void sumForces(){
     //attractive force
-    int Fca=10;
-    double distance = sqrt(pow((double)abs(p_obj[0] - p[0]), 2) + pow((double)abs(p_obj[1] - p[1]), 2));
-    double aFx = Fca*(p_obj[0]-p[0])/distance;
-    double aFy = Fca*(p_obj[1]-p[1])/distance;
+    float rFx=0.0;
+    float rFy=0.0;
+    float distance = sqrt(pow((float)abs(p_final[1] - p[1]), 2) + pow((float)abs(p_final[0] - p[0]), 2));
+    float aFx = Fca*(p_final[0]-p[0])/distance;
+    float aFy = Fca*(p_final[1]-p[1])/distance;
     //repulsive force
-    double rFx=0;
-    double rFy=0;
     for(int i=0;i<aSize;i++){
         for(int j=0;j<aSize;j++){
             rFx+=activeReg[i][j].forceX;
@@ -203,4 +207,5 @@
     //sum
     fX=aFx-rFx;
     fY=aFy-rFy;
+    pc.printf("Repulsive (X): X=%lf   Force(Y)=%lf\n\n", rFx, rFy);
 }
\ No newline at end of file