Revision 4:6ebe8982de0e, committed 2019-05-06

Comitter:

jsobiecki

Date:

Mon May 06 17:15:52 2019 +0000

Parent:

3:0a718d139ed1

Child:

5:5653e559a67b

Commit message:

add lidar

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ActiveCell.h	Mon May 06 17:15:52 2019 +0000
@@ -0,0 +1,71 @@
+#ifndef UNTITLED2_ACTIVECELL_H
+#define UNTITLED2_ACTIVECELL_H
+#include <iostream>
+#include <iomanip>
+#include <math.h>
+
+class ActiveCell {
+public:
+    //cell dimension = 5x5cm
+    int cellVal;
+    float forceX;
+    float forceY;
+    float distance;
+    int xt, yt;
+    static const float repulsiveForce = 20;
+    //robots position
+    int x0;
+    int y0;
+    ///////////VFH//////////////
+    static const int a=70,b=2;
+    float angle;
+    float amplitude;
+    int sectorK;
+    //resolution - if changing change also secVal in main & calcSectors
+    static const int res=10;
+
+    ActiveCell(){
+        angle=0;
+        amplitude=0;
+        sectorK=0;
+        cellVal=0;
+        forceX=0;
+        forceY=0;
+        x0=0;
+        y0=0;
+    }
+
+
+    void calForce() {
+        forceX = (repulsiveForce * cellVal / pow(distance, 2)) * (xt - x0) / distance;
+        forceY = (repulsiveForce * cellVal / pow(distance, 2)) * (yt - y0) / distance;
+        amplitude = cellVal*cellVal*(a-b*distance);
+    }
+//Calculating distance from the robot 
+    void calDist(int idX, int idY) {
+        if (idX < 5) {
+            xt = x0 - (5 - idX) * 5;
+        } else if (idX == 5) {
+            xt = x0;
+        } else {
+            xt = x0 + (idX - 5) * 5;
+        }
+        if (idY > 5) {
+            yt = y0 - (5 - idY) * 5;
+        } else if (idY == 5) {
+            yt = y0;
+        } else {
+            yt = y0 + (idY - 5) * 5;
+        }
+        distance = sqrt(pow((float)abs(x0 - xt), 2) + pow((float)abs(y0 - yt), 2));
+        
+        //////angle/////////////
+        angle = atan2((float)yt-y0,(float)xt-x0)*180/3.14159265358979323846f;
+        sectorK=angle/res;
+        if(sectorK<0)
+            sectorK=36+sectorK;
+    }
+};
+
+
+#endif //UNTITLED2_ACTIVECELL_H

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HistogramCell.h	Mon May 06 17:15:52 2019 +0000
@@ -0,0 +1,20 @@
+#ifndef UNTITLED2_HISTOGRAMCELL_H
+#define UNTITLED2_HISTOGRAMCELL_H
+
+
+class HistogramCell {
+public:
+    int cellVal;
+    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;
+    }
+};
+
+
+#endif //UNTITLED2_HISTOGRAMCELL_H

--- a/main.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/main.cpp	Mon May 06 17:15:52 2019 +0000
@@ -1,15 +1,44 @@
-// Coded by Luís Afonso 11-04-2019
+    // Coded by Luís Afonso 11-04-2019
 #include "mbed.h"
 #include "BufferedSerial.h"
 #include "rplidar.h"
 #include "Robot.h"
 #include "Communication.h"
+#include "math.h"
+#include "ActiveCell.h"
+#include "HistogramCell.h"
+#define M_PI 3.14159265358979323846f
 
-Serial pc(SERIAL_TX, SERIAL_RX);
 RPLidar lidar;
 BufferedSerial se_lidar(PA_9, PA_10);
 PwmOut rplidar_motor(D3);
 
+//EXERCICIO 1
+//Luis Cruz N2011164454
+//Jacek Sobecki N2018319609
+Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+DigitalIn button(PC_13);
+void poseEst(float p[], float radius, float enc_res, float b);
+void SpeedLim(float w[]);
+void initializeArrays();
+void calcSectors(float theta);
+void sumForces();
+void updateActive(float xR, float yR,float theta);
+//int ReadSensors();
+//const int m = 200, n = 200, activeSize = 11;
+//histogram size | aSize active region size
+const int hSize = 80, aSize = 11;
+ActiveCell activeReg[aSize][aSize];
+HistogramCell histogram[hSize][hSize];
+//Repulsive force sums
+float p[3], p_obj[3], p_final[3], fX, fY;
+const float Fca=6;/*5*/
+
+//VFH
+const int L=2;
+float secVal[36];
+float smooth[36];
+
 int main()
 {
     float odomX, odomY, odomTheta;
@@ -24,15 +53,463 @@
     lidar.setAngle(0,360);
 
     pc.printf("Program started.\n");
-        
+    
     lidar.startThreadScan();
     
-    while(1) {
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    button.mode(PullUp);
+    getCountsAndReset();
+    setSpeeds(0, 0);
+    initializeArrays();
+    while(button==1);
+    rplidar_motor.write(0.7f);
+    //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, 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 = 5, k_f = 12.5; /*12.5*/ //VFF
+    float theta_final;
+// ===============================================================================
+// =================================== COORDS ====================================
+// =============================================================================== 
+    //Target coordinates
+    p_final[0] = 100, p_final[1] = 20, 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;
+// ===============================================================================
+// =================================== EXECUTION =================================
+// ===============================================================================
+    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("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+                pc.printf("dist:%f  angle:%f   %d   %c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+        }
+        
+        
+        getCountsAndReset();
+        //pc.printf("Speeds: Left=%lf   Right=%lf\n", w[1], w[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);
+        
+        
+        //------------Process lidar readings
+        //theta of the robot in degrees
+        float thetaR_deg = 0;
+        //float thetaR_deg = (p[2]*180.0f)/M_PI;
+        if(thetaR_deg <0) thetaR_deg=360+thetaR_deg;
+        //real angle of the reading
+        if(data.angle<360 && data.angle>0) data.angle=360-data.angle;
+        float readAngle = data.angle - 180 + thetaR_deg;
+        if(readAngle>=360) readAngle=fmod(readAngle,360);
+        
+        pc.printf("Robots_deg: %f  data_deg: %f  readAngle: %f\n",thetaR_deg,data.angle,readAngle);
+        
+        //update cells according to a reading
+        if(data.distance!=0 && data.distance<200){
+        for(int i=0;i<11;i++){
+            for (int j = 0; j < 11; j++) {
+                if(readAngle<=activeReg[i][j].sectorK*10+5 && readAngle>=activeReg[i][j].sectorK*10-5){
+                    if(data.distance<=((double)activeReg[i][j].distance+2.5)*10 && data.distance>=((double)activeReg[i][j].distance-2.5)*10) {
+                        //cell is occupied
+                        activeReg[i][j].cellVal = 3;
+                    } else if (data.distance>((double)activeReg[i][j].distance-2.5)*10){
+                            //cell is unoccupied
+                            activeReg[i][j].cellVal = 1;
+                    }
+                }
+            }
+        }
+        }
+        
+        for (int j = 10; j >= 0; j--) {
+        for (int i = 0; i < 11; i++) {
+            cout << "[" << activeReg[i][j].cellVal << "]";
+        }
+        cout << endl;
+    }
+        
+  
+        //Path calculation
+        poseEst(p, radius, enc_res, b); //Pose estimation
+        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;
+        
+        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])) < 5){
+            setSpeeds(0,0);
+        }
+        else{
+            //setSpeeds(w[1], w[2]);
+            }
+        wait(sample_time); 
+    }
+}
+// ===============================================================================
+// =================================== FUNCTIONS =================================
+// ===============================================================================
+//Pose Estimation function
+void poseEst(float p[], float radius, float enc_res, float b){
+    float deltaDl, deltaDr, deltaD, deltaT;
+    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;
+        p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
+        return;
+    }
+    p[0] = p[0] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*cos(p[2]+deltaT/2.0f);
+    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;
+    wratio = fabs(w[2]/w[1]);
+    if(w[2] > 150 || w[1] > 150){
+        if(wratio < 1){
+            w[1] = 150;
+            w[2] = w[1]*wratio;
+        }
+        else if(wratio > 1){
+            w[2] = 150;
+            w[1] = w[2]/wratio;
+        }
+        else{
+            w[2] = 150;
+            w[1] = 150;
+        }
+    }
+    if(w[2] < 50 || w[1] < 50){
+        if(wratio < 1){
+            w[1] = 50;
+            w[2] = w[1]*wratio;
+        }
+        else if(wratio > 1){
+            w[2] = 50;
+            w[1] = w[2]/wratio;
+        }
+        else{
+            w[2] = 50;
+            w[1] = 50;
+        }
+    }
+}
+
+void initializeArrays() {
+    for (int i = 0; i < hSize; i++) {
+        for (int j = 0; j < hSize; j++) {
+            histogram[i][j].calculate(i, j);
+            //if(((i >= 8 && i <= 12) && (j == 0 || j == 8)) || ((i == 8 || i == 12) && (j >= 0 && j <= 8))) histogram[i][j].cellVal=3;
+            //if(((i >= 0 && i <= 3) && (j == 8 || j == 12)) || ((i == 0 || i == 3) && (j >= 8 && j <= 12))) histogram[i][j].cellVal=3;
+            //if(((i >= 14 && i <= 20) && (j == 8 || j == 9)) || ((i == 14 || i == 20) && (j >= 8 && j <= 9))) histogram[i][j].cellVal=3;
+        }
+    }
+    for (int i = 0; i < aSize; i++) {
+        for (int j = 0; j < aSize; j++) {
+            activeReg[i][j].calDist(i, j);
+        }
+    }
+}
+
+//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(float xR, float yR,float theta) {
+    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) {
+                idXr = i;
+                idYr = j;
+                break;
+            }
+        }
+    }
+    int m = idXr - aSize / 2;
+    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) {
+                histogram[m][n].cellVal=activeReg[k][l].cellVal;
+            }
+            n++;
+        }
+        m++;
+    }
+    for (int i = 0; i < aSize; ++i) {
+        for (int j = 0; j < aSize; ++j) {
+            activeReg[i][j].calForce();
+        }
+    }
+    activeReg[5][5].amplitude=0;
+    activeReg[5][5].amplitude=0;
+    
+    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){
+    for (int k = 0; k < 36; ++k) {
+        secVal[k]=0;
+        for (int i = 0; i < aSize; ++i) {
+            for (int j = 0; j < aSize; ++j) {
+                if(activeReg[i][j].sectorK==k)
+                    secVal[k]+=activeReg[i][j].amplitude;
+            }
         }
-       wait(0.01); 
+    }
+
+    smooth[0]=(secVal[34]+2*secVal[35]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
+    smooth[1]=(secVal[35]+2*secVal[0]+2*secVal[1]+2*secVal[2]+secVal[3])/5;
+    smooth[34]=(secVal[32]+2*secVal[33]+2*secVal[34]+2*secVal[35]+secVal[0])/5;
+    smooth[35]=(secVal[33]+2*secVal[34]+2*secVal[35]+2*secVal[0]+secVal[1])/5;
+    for (int i = 2; i < 34; ++i) {
+        smooth[i]=(secVal[i-L]+2*secVal[i-L+1]+2*secVal[i]+2*secVal[i+L-1]+secVal[i+L])/5;
+    }
+    
+    const int thresh=200;//100
+    int temp[36];
+    int counter = 0, aux = 0;
+    int valley[36];
+    for(int i=0;i<36;++i){
+        //pc.printf("|%lf", smooth[i]);
+        if(smooth[i]<thresh){
+            temp[i]=1;
+            //valley[aux][aux] = 
+            counter++;
+        }
+        else{
+            valley[aux] = counter;
+            counter = 0;
+            aux++;
+            temp[i]=0;
+            //pc.printf("#%d", i);
+        }
+        
+    }
+    float best=999;
+    float theta_deg;
+    theta_deg =(theta*180.0f)/M_PI;
+    //pc.printf("theta (degrees): = %lf\n\n", theta_deg);
+   int destSec = theta_deg / 10;
+   if(destSec<0) destSec=36+destSec;
+   //cout<<"destination sector: "<<destSec<<endl;
+   
+   int L=destSec;
+    int R=destSec;
+    while(temp[L]==0){
+        L--;
+        if(L<0) L=35;
+    }
+    while(temp[R]==0){
+        R++;
+        if(R>35) R=0;
     }
-}
\ No newline at end of file
+   
+    float dirSet, dirC,dirL,dirR;
+    if(temp[destSec]==1){
+        int k=destSec-1;
+        if(k<0) k=35;
+        int size=1;
+        while(temp[k]==1){
+            size++;
+            k--;
+            if(k<0) k=35;
+            if(k==destSec) break;
+            if(size>=5) break;
+        }
+        int right=k+1;
+        if(right<0) right=35;
+        k=destSec+1;
+        if(k>35) k=0;
+        while(temp[k]==1){
+            size++;
+            k++;
+            if(k>35) k=0;
+            if(k==destSec) break;
+            if(size>=5) break;
+        }
+        int left=k-1;
+        if(left>35) left=0;
+        if(size>=5) {
+        //wide
+            dirC=destSec*10;
+            //cout << "wide"<<endl;
+            }
+    
+        else if(size>4 && size<5) //narrow
+        {
+            dirC=0.5*(left*10+right*10);
+            //cout<<"narrow"<<endl;
+        } else {
+            int secL = L;
+        while (temp[secL] != 1) {
+            secL++;
+            if (secL > 35) secL = 0;
+        }
+        int rightL = secL;
+        int size = 1;
+
+        int i = secL + 1;
+        if (i > 35) i = 0;
+        while (temp[i] == 1) {
+            size++;
+            i++;
+            if (i > 35) i = 0;
+            if (i == secL) break;
+            // Smax here
+            if (size >= 5) break;
+        }
+        int leftL = i - 1;
+        if (leftL < 0) leftL = 35;
+        if (size >= 5) //wide
+            dirL = rightL * 10 + 0.5 * 10 * 5;
+        else if(size>4 && size<5)  //narrow
+            dirL = 0.5 * (leftL * 10 + rightL * 10);
+        else
+            dirL=9999;
+        ///////////////////////////////////////////////////////////////////
+        int secR = R;
+        while (temp[secR] != 1) {
+            secR--;
+            if (secR < 0) secR = 35;
+        }
+
+        int leftR = secR;
+        int sizeR = 1;
+
+        int j = secR - 1;
+        if (j < 0) j = 35;
+        while (temp[j] == 1) {
+            sizeR++;
+            j--;
+            if (j < 0) j = 35;
+            if (j == secR) break;
+            if (sizeR >= 5) break;
+        }
+        int rightR = j + 1;
+        if (rightR > 35) rightR = 0;
+        if (sizeR >= 5) //wide
+            dirR = leftR * 10 + 0.5 * 10 * 5;
+        else if(sizeR>4 && sizeR<5)//narrow
+            dirR = 0.5 * (rightR * 10 + leftR * 10);
+        else
+            dirR=9999;
+
+        if(dirL>360) dirL=fabs(dirL-360);
+        if(dirR>360) dirR=fabs(dirR-360);
+        if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
+            dirC=dirR;
+        else
+            dirC=dirL;
+        }
+        dirSet=dirC;
+        //cout<<"dirSet: 1"<<endl;
+
+        ///////////////////////////////////////////////////////////
+    } else {
+        int secL = destSec;
+        while (temp[secL] != 1) {
+            secL++;
+            if (secL > 35) secL = 0;
+        }
+        int rightL = secL;
+        int size = 1;
+
+        int i = secL + 1;
+        if (i > 35) i = 0;
+        while (temp[i] == 1) {
+            size++;
+            i++;
+            if (i > 35) i = 0;
+            if (i == secL) break;
+            // Smax here
+            if (size >= 5) break;
+        }
+        int leftL = i - 1;
+        if (leftL < 0) leftL = 35;
+        if (size >= 5) //wide
+            dirL = rightL * 10 + 0.5 * 10 * 5;
+        else if(size>4 && size<5)  //narrow
+            dirL = 0.5 * (leftL * 10 + rightL * 10);
+        else
+            dirL=9999;
+        ///////////////////////////////////////////////////////////////////
+        int secR = destSec;
+        while (temp[secR] != 1) {
+            secR--;
+            if (secR < 0) secR = 35;
+        }
+
+        int leftR = secR;
+        int sizeR = 1;
+
+        int j = secR - 1;
+        if (j < 0) j = 35;
+        while (temp[j] == 1) {
+            sizeR++;
+            j--;
+            if (j < 0) j = 35;
+            if (j == secR) break;
+            if (sizeR >= 5) break;
+        }
+        int rightR = j + 1;
+        if (rightR > 35) rightR = 0;
+        if (sizeR >= 5) //wide
+            dirR = leftR * 10 + 0.5 * 10 * 5;
+        else if(sizeR>4 && sizeR<5)//narrow
+            dirR = 0.5 * (rightR * 10 + leftR * 10);
+        else
+            dirR=9999;
+
+        if(dirL>360) dirL=fabs(dirL-360);
+        if(dirR>360) dirR=fabs(dirR-360);
+        if(fabs(theta_deg-dirL)>fabs(theta_deg-dirR))
+            dirSet=dirR;
+        else
+            dirSet=dirL;
+        //cout<<"dirSet:2 dirR: "<<dirR<<" dirL: "<<dirL<<endl;
+    }
+    //cout<<"dirSet: "<<dirSet<<endl;
+    
+    fX=cos(dirSet*M_PI/180.0f);
+    fY=sin(dirSet*M_PI/180.0f);
+    
+}