File content as of revision 0:25f4809c2729:

#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);
}