File content as of revision 3:a0b0e4acf5e9:

#include "mbed.h"
#include "Robot.h"
#include "math.h"
#define M_PI 3.14159265358979323846
//EXERCICIO 1
//Luis Cruz N2011164454
//Jacek Sobecki 
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[]);
//int ReadSensors();
const int n = 200, activeSize = 11;
void updateHist(int hist[][n], int active[][activeSize],int x,int y);
int main(){

    button.mode(PullUp);
    getCountsAndReset();
    setSpeeds(0, 0);
    while(button==1);

    //w[0] = Omega     | w[1] = X     | w[2] = Y
    //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 |
    const int m = 200;
    int hist[m][n] , active[activeSize][activeSize];
    float  w[3], v, p[3], p_obj[3], 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 = 0.0, sample_time = 0.05, Fcr = 1.0, d_stalker = 3.0;
// ===============================================================================
// =================================== COORDS ====================================
// =============================================================================== 
    //Target coordinates
    p_obj[0] = 200, p_obj[1] = 200, p_obj[2] = 0;
    //Initial coordinates:
    p[0] = 100, p[1] = 100, 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\n", p[0], p[1], p[2]);
        
        //Path calculation
        poseEst(p, radius, enc_res, b); //Pose estimation
        //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]);
        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); //direction gain
        integral += err;
        v = k_v*err+k_i*integral; //Speed gain
        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){
            pc.printf("\n YOU REACHED YOUR DESTINATION, THANK YOU FOR CHOOSING EMIRATES\n");
            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.0*M_PI*radius/enc_res);
    deltaDr = ((float)countsRight)*(2.0*M_PI*radius/enc_res);
    deltaD = (deltaDr + deltaDl)/2;
    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 = 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] < 35 || w[1] < 35){
        if(wratio < 1){
            w[1] = 35;
            w[2] = w[1]*wratio;
        }
        else if(wratio > 1){
            w[2] = 35;
            w[1] = w[2]/wratio;
        }
        else{
            w[2] = 35;
            w[1] = 35;
        }
    }
}
void updateHist(int hist[][n], int active[][activeSize],int x,int y){
    int aX=0;
    for(int i=x-(activeSize/2);i<x+(activeSize/2+1);i++){
        int aY=0;
        for(int j=y-(activeSize/2);j<y+(activeSize/2+1);j++){
            if(i>=0 && j>=0){
                hist[i][j]=active[aX][aY];
                aY++;
            }
        }
        aX++;
    }
}