Diff: main.cpp

Revision:

0:25f4809c2729

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,213 @@
+#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
+// LABWORK 4 v0.1
+// LUIS CRUZ 2011164454
+// JACEK SOBECKI
+RPLidar lidar;
+BufferedSerial se_lidar(PA_9, PA_10);
+PwmOut rplidar_motor(D3);
+struct RPLidarMeasurement data;
+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();
+int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]);
+void observe();
+
+const int hSize = 80, aSize = 11;//
+ActiveCell activeReg[aSize][aSize];
+HistogramCell histogram[hSize][hSize];
+
+int main(){
+
+    button.mode(PullUp);
+    getCountsAndReset();
+    setSpeeds(0, 0);
+    initializeArrays();
+    while(button==1);
+    rplidar_motor.period(0.001f);
+    lidar.begin(se_lidar);
+    lidar.setAngle(0,360);
+    rplidar_motor.write(0.9f);
+    pc.printf("Program started.\n");
+    lidar.startThreadScan();
+    //w[0] = Omega     | w[1] = Left  | w[2] = Right
+    //p[0] = X         | p[1] = Y     | p[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, p[3], p_obj[3], theta, theta_error;
+    const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/, 
+    k_s = 12/*10*/, sample_time = 0.05;
+// ===============================================================================
+// =================================== COORDS ====================================
+// =============================================================================== 
+    //Target coordinates
+    p_obj[0] = 100, p_obj[1] = 20, p_obj[2] = 0;
+    //Initial coordinates:
+    p[0] = 20, p[1] = 20, p[2] = 0;
+// ===============================================================================
+// =================================== EXECUTION =================================
+// ===============================================================================
+    while(1){
+        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]);
+        if(lidar.pollSensorData(&data) == 0) pc.printf("dist:%f  angle:%f\n", data.distance, data.angle); // Prints one lidar measurement.
+        //Path calculation
+        poseEst(p, radius, enc_res, b);
+        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); //DEBUG
+        v = k_v*sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2));
+        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])) < 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] += deltaD*cos(p[2]+deltaT/2.0f);
+    p[1] += deltaD*sin(p[2]+deltaT/2.0f);
+    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;
+        }
+    }
+}
+//Initialize apriori knowledge of map
+void initializeArrays(){
+    for (int i = 0; i < aSize; i++) {
+        for (int j = 0; j < aSize; j++) {
+            activeReg[i][j].calDist(i, j);
+        }
+    }
+}
+//Bresenham algo -> trace the Lidar's reading line into a bitmap a.k.a. cell's index
+int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]){
+    int aux = 0;
+    int delta_x(x2 - x1);
+    // if xR == x2, then it does not matter what we set here
+    signed char const ix((delta_x > 0) - (delta_x < 0));
+    delta_x = std::abs(delta_x) << 1;
+    int delta_y(y2 - y1);
+    // if yR == y2, then it does not matter what we set here
+    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 | xF = %d | yF = %d | aux = %d \n", x1, y1, x2, y2, aux-1);
+    if (delta_x >= delta_y){
+        // error may go below zero
+        int error(delta_y - (delta_x >> 1));
+        while (x1 != x2)
+        {
+            // reduce error, while taking into account the corner case of error == 0
+            if ((error > 0) || (!error && (ix > 0))){
+                error -= delta_x;
+                y1 += iy;
+            }
+            error += delta_y;
+            x1 += ix;
+            cX[aux] = x1, cY[aux] = y1, aux++;
+        }
+    }
+    else{
+        // error may go below zero
+        int error(delta_x - (delta_y >> 1));
+        while (y1 != y2){
+            // reduce error, while taking into account the corner case of error == 0
+            if ((error > 0) || (!error && (iy > 0))){
+                error -= delta_y;
+                x1 += ix;
+            }
+            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
+        }
+    }
+    return aux;
+}
+void observe(){
+    float thetaR_deg, readAngle, lDist, lAng;
+    int xR, yR, xL, yL, xF, yF;
+    //------------Processing LIDAR readings------------
+    lDist = data.distance / 10; //mm -> cm
+    if(lDist == 0 || lDist > 2000) return;
+    lAng = data.angle;
+    thetaR_deg = (p[2] * 180.0f) / M_PI;
+    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]); //DEBUG
+    readAngle = (readAngle * M_PI) /180.0f; // deg -> rads
+    xL = lDist * cos(readAngle)/5;//X_coord Lidar's reading (cell unit)
+    yL = lDist * sin(readAngle)/5;//Y_coord Lidar's reading (cell unit)
+    xR = p[0]/5, yR = p[1]/5; //Robot's coordinates cell
+    xF = xR + xL;
+    yF = yR + yL;
+    //if(xF < 0 || yF < 0 || xF > 80 || yF > 80) return;
+    npts = Bresenham(xR, yR, xF, yF, cX, cY);
+}
\ No newline at end of file