Revision 7:5e59f8a011fd, committed 2019-06-07

Comitter:

shut

Date:

Fri Jun 07 15:39:06 2019 +0000

Parent:

6:1c84602323c8

Commit message:

7-6

Changed in this revision

--- a/main.cpp	Tue May 14 17:15:43 2019 +0000
+++ b/main.cpp	Fri Jun 07 15:39:06 2019 +0000
@@ -33,6 +33,7 @@
 HistogramCell histogram[hSize][hSize];
 //Repulsive force sums
 float p[3], p_obj[3], p_final[3], fX, fY;
+int aux99 = 0;
 //const float Fca=6;/*5*/
 
 //VFH
@@ -52,7 +53,7 @@
     rplidar_motor.period(0.001f);
     lidar.begin(se_lidar);
     lidar.setAngle(0,360);
-    rplidar_motor.write(1.0f);
+    rplidar_motor.write(0.8f);
     pc.printf("Program started.\n");
     lidar.startThreadScan();
     //w[0] = Omega     | w[1] = Left  | w[2] = Right
@@ -69,19 +70,19 @@
 // =================================== COORDS ====================================
 // =============================================================================== 
     //Target coordinates
-    p_final[0] = 100, p_final[1] = 20, p_final[2] = 0;
+    p_final[0] = 250, p_final[1] = 100, p_final[2] = 0;
     //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
     //Initial coordinates:
-    p[0] = 20, p[1] = 20, p[2] = 0;
+    p[0] = 100, p[1] = 100, p[2] = 0;
 // ===============================================================================
 // =================================== EXECUTION =================================
 // ===============================================================================
     initializeArrays();
     while(1){
         // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
-        if(lidar.pollSensorData(&data) == 0) pc.printf("dist:%f  angle:%f\n", data.distance, data.angle); // Prints one lidar measurement.
+        if(lidar.pollSensorData(&data) == 0) //pc.printf("dist:%f  angle:%f\n", data.distance, data.angle); // Prints one lidar measurement.
         getCountsAndReset();
-        pc.printf("Speeds: Left=%lf   Right=%lf\n", w[1], w[2]);  //    DEBUG
+        //pc.printf("Speeds: Left=%lf   Right=%lf\n", w[1], w[2]);  //    DEBUG
         //pc.printf("OBJECTIVE X: %lf  OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]);        DEBUG
         pc.printf("Position: X=%lf   Y=%lf   Theta=%lf\n\n", p[0], p[1], p[2]);//       DEBUG
         //pc.printf("Force (X): X=%lf   Force(Y)=%lf\n", fX, fY);       DEBUG
@@ -112,10 +113,11 @@
         if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 5){
             setSpeeds(0,0);
         }
-        else{
+        else if (aux99 > 5){
             setSpeeds(w[1], w[2]);// Motor's output
             }
         wait(sample_time); 
+        aux99++;
     }
 }
 // ===============================================================================
@@ -137,6 +139,9 @@
     p[1] = p[1] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*sin(p[2]+deltaT/2.0f);
     p[2] = p[2] + deltaT;
 }
+
+
+
 //Speed limiter function
 void SpeedLim(float w[]){
     float wratio;
@@ -155,18 +160,18 @@
             w[1] = 150;
         }
     }
-    if(w[2] < 50 || w[1] < 50){
+    if(w[2] < 70 || w[1] < 70){
         if(wratio < 1){
-            w[1] = 50;
+            w[1] = 70;
             w[2] = w[1]*wratio;
         }
         else if(wratio > 1){
-            w[2] = 50;
+            w[2] = 70;
             w[1] = w[2]/wratio;
         }
         else{
-            w[2] = 50;
-            w[1] = 50;
+            w[2] = 70;
+            w[1] = 70;
         }
     }
 }
@@ -192,8 +197,8 @@
     int idYr = 0;
     for (int i = 0; i < hSize; i++) {
         for (int j = 0; j < hSize; j++) {
-            if (xR > (i*5.0f+2.5f - 2.5f) && xR < (i*5.0f+2.5f + 2.5f) && yR > (j*5.0f+2.5f - 2.5f) &&
-                yR < (j*5.0f+2.5f + 2.5f)){
+            if (xR >= (i*5.0f) && xR < (i*5.0f+5.0f) && yR >= (j*5.0f) &&
+                yR < (j*5.0f+5.0f)){
                 idXr = i;
                 idYr = j;
                 break;
@@ -220,12 +225,12 @@
     }
     activeReg[5][5].amplitude=0;
     activeReg[5][5].amplitude=0;
-    for (int j = 10; j >= 0; j--) {
+    /*for (int j = 10; j >= 0; j--) {
         for (int i = 0; i < 11; i++) {
             cout << "[" << activeReg[i][j].cellVal << "]";
         }
         cout << endl;
-    }
+    }*/
     calcSectors(theta);
 }
 void calcSectors(float theta){
@@ -338,7 +343,7 @@
             if (i > 35) i = 0;
             if (i == secL) break;
             // Smax here
-            if (size >= 5) break;
+            if (size >= 5) break; //tried 10, same behaviour
         }
         int leftL = i - 1;
         if (leftL < 0) leftL = 35;
@@ -386,7 +391,7 @@
         dirSet=dirC;
         //cout<<"dirSet: 1"<<endl;
 
-        ///////////////////////////////////////////////////////////
+    ///////////////////////////////////////////////////////////
     } else {
         int secL = destSec;
         while (temp[secL] != 1) {
@@ -404,7 +409,7 @@
             if (i > 35) i = 0;
             if (i == secL) break;
             // Smax here
-            if (size >= 5) break;
+            if (size >= 5) break; //5
         }
         int leftL = i - 1;
         if (leftL < 0) leftL = 35;
@@ -458,26 +463,27 @@
 
 void mapping(){
     float thetaR_deg, readAngle, lDist, lAng;
-    int xR, yR, xL, yL, cX[128], cY[128], npts;
+    int xR, yR, xL, yL, cX[400], cY[400], npts, xF, yF;
     //------------Processing LIDAR readings------------
     lDist = data.distance / 10; //mm -> cm
-    if(lDist == 0 || lDist > 400) return; //TO DO (opt): if less than ~10cm! -> make a turn around 
+    if(lDist == 0 || lDist > 200) return; //TO DO (opt): if less than ~10cm! -> make a turn around 
     lAng = data.angle;
     thetaR_deg = (p[2] * 180.0f) / M_PI; //TO DO: Add the robot's angle in world frame to the lidar's reagin angle (readAngle)
     if(thetaR_deg < 0) thetaR_deg = 360 + thetaR_deg;
     readAngle = 270 - lAng + thetaR_deg; //Align LIDAR's frame with robot's one
     if(readAngle > 360) readAngle -= 360; //   "     "      "      "      "
     else if(readAngle < 0) readAngle += 360;// "     "      "      "      "
-    pc.printf("ReadAngle: %f | data_deg: %f | Distance: %f | pos: (%f,%f)\n", readAngle, lAng, lDist, p[0], p[1]);
+    //pc.printf("ReadAngle: %f | data_deg: %f | Distance: %f | pos: (%f,%f)\n", readAngle, lAng, lDist, p[0], p[1]);
     readAngle = (readAngle * M_PI) /180.0f; // deg -> rads
     xL = lDist * cos(readAngle);
     yL = lDist * sin(readAngle);
     xR = p[0]/5, yR = p[1]/5, xL = xL/5, yL = yL/5; // cm -> cell index units
-    int xF = xR + xL; //(xR - xL) -> delta distance THINK IS WRONG should be delta = yL-yR;
-    int yF = yR + yL;//(yR - yL) -> delta distance  THINK IS WRONG should be delta = yL-yR;
-    pc.printf("xL: %d | yL: %d", xF, yF);
+    xF = xR + xL; //(xR - xL) -> delta distance THINK IS WRONG should be delta = yL-yR;
+    yF = yR + yL;//(yR - yL) -> delta distance  THINK IS WRONG should be delta = yL-yR;
+    //if(xF < 0 || yF < 0 || xF > 80 || yF > 80) return;
+    //pc.printf("xF: %d | yF: %d", xF, yF);
     npts = Bresenham(xR, yR, xF, yF, cX, cY);
-    //log_cell(cX, cY, npts);
+    log_cell(cX, cY, npts);
     prob_calc();
 }
 //Bresenham algo -> trace the Lidar's reading line into a bitmap a.k.a. cell's index
@@ -492,7 +498,7 @@
     signed char const iy((delta_y > 0) - (delta_y < 0));
     delta_y = std::abs(delta_y) << 1;
     cX[aux] = x1, cY[aux] = y1, aux++;
-    pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
+    //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
     if (delta_x >= delta_y){
         // error may go below zero
         int error(delta_y - (delta_x >> 1));
@@ -506,7 +512,7 @@
             error += delta_y;
             x1 += ix;
             cX[aux] = x1, cY[aux] = y1, aux++;
-            pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
+            //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1);
         }
     }
     else{
@@ -521,7 +527,7 @@
             error += delta_x;
             y1 += iy;
             cX[aux] = x1, cY[aux] = y1, aux++;
-            //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1); DEBUG
+            //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1); 
         }
     }
     return aux;
@@ -530,9 +536,15 @@
     float l_occ = 0.65;
     float l_free = -0.65;
     for(int i = 0; i < (npts-1); i++){
+        if(cX[i] < 0 || cY[i] < 0) break;
         histogram[cX[i]][cY[i]].logodds = histogram[cX[i]][cY[i]].logodds + l_free; //l0 já inicializado com o array
     }
-    histogram[cX[npts-1]][cY[npts-1]].logodds = histogram[cX[npts]][cY[npts]].logodds + l_occ;
+    histogram[cX[npts-1]][cY[npts-1]].logodds = histogram[cX[npts-1]][cY[npts-1]].logodds + l_occ;
+    /*for (int i = -1; i < 2; i++){
+        for (int j = -1; j < 2; j++){
+            histogram[cX[npts-1]+i][cY[npts-1]+j].logodds = histogram[cX[npts-1]+i][cY[npts-1]+j].logodds + l_occ;
+        }
+    }*/
 //Força precisa de ser calculada de log odds para probabilidades: <50% de ocupação = livre
 //Se o valor da repulsiva óptimo era 3, então vai oscilar entre 0 e 3 - 50 e 100%
 }
@@ -542,7 +554,7 @@
         for (int j = 0; j < hSize; j++) {
             aux = (1.0f - (1.0f/(1.0f+exp(histogram[i][j].logodds))));
             if(aux > 0.5f){
-                histogram[i][j].cellVal = 3*aux;
+                histogram[i][j].cellVal = 20*aux; //6
             }
         }
     }