File content as of revision 10:32c65de8ff37:

#include "mbed.h"
#include "Robot.h"
#include "math.h"
#include "ActiveCell.h"
#include "HistogramCell.h"
#define M_PI 3.14159265358979323846f
//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(){

    button.mode(PullUp);
    getCountsAndReset();
    setSpeeds(0, 0);
    initializeArrays();
    while(button==1);
    //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; /*7.5*/ //VFF
    float theta_final;
// ===============================================================================
// =================================== COORDS ====================================
// =============================================================================== 
    //Target coordinates
    p_final[0] = 200, 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){
        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);
        
        //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;
        
pc.printf("w0 = %lf | w1 = %lf | w2 = %lf\n",  w[0], w[1], w[2]);
        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])) < 4){
            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>18 && i<22 && j > 3 && j<5) histogram[i][j].cellVal=5;
        }
    }
    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.5f && xR < histogram[i][j].x + 2.5f && yR > histogram[i][j].y - 2.5f &&
                yR < histogram[i][j].y + 2.5f) {
                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) {
                activeReg[k][l].cellVal = histogram[m][n].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;
    
    calcSectors(theta);
}
void calcSectors(float theta){
    for (int k = 0; k < 36; ++k) {
        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;
            }
        }
    }

    smooth[0]=(secVal[28]+2*secVal[29]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
    smooth[1]=(secVal[29]+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=100;
    int temp[36];
    for(int i=0;i<36;++i){
        if(smooth[i]<thresh)
            temp[i]=1;
        else
            temp[i]=0;
    }
    float best=999;
    float theta_deg;
    theta_deg =theta*180.0f/3.14159265358979323846f;
    if(theta_deg<0) theta_deg=360.0f+theta_deg;
    pc.printf("theta corrr: = %lf\n\n", theta_deg);
    for (int i = 0; i < 36; ++i) {
        if(temp[i]!=0 && fabs(10*i-theta_deg)<fabs(10*best-theta_deg)) best=i;
    }
    pc.printf("best = %lf\n\n", best);
    fX=cos(best*10.0f*M_PI/180.0f);
    fY=sin(best*10.0f*M_PI/180.0f);
    
}