File content as of revision 3:892592b2c2ea:

#include "mbed.h"
#include "BufferedSerial.h"
#include "rplidar.h"
#include "Robot.h"
#include "Communication.h"
#include <math.h>
#include <time.h>

#define M_PI 3.14159265358979323846

Serial pc(SERIAL_TX, SERIAL_RX);
RPLidar lidar;
BufferedSerial se_lidar(PA_9, PA_10);
PwmOut rplidar_motor(D3);

clock_t start, end;

int nW=2;
float wTheta[2] = {0, 0};
float wRad[2] = {3.5, 3.5};
float wWid[2] = {1.5, 1.5};
float wheelsRadius = 3.5;
float wheelsDistance = 14.7;

//Platform Properties
float platRad=18/2;   

//Constante de Tempo
double T = 0;

//Definição do Mapa
float Mapa200[200][200] = {0.5};
float Mapa40[40][40]={0.5};
int log_mapa[40][40]={0};


//Size Map40
int X = ceil(200/5);
int Y = ceil(200/5);

/*
%Parede da esquerda
for(int i = 0; i <= 200; i++){
   Mapa200[i][0] = {1};
   Mapa200[i][1] = {1};
   Mapa200[i][2] = {1};
}

%Parede da direita
for(int i = 0; i <= 200; i++){
   Mapa200[i][199] = {1};
   Mapa200[i][198] = {1};
   Mapa200[i][197] = {1};
}

%Parede de cima
for(int i = 0; i <= 200; i++){
   Mapa200[0][i] = {1};
   Mapa200[1][i] = {1};
   Mapa200[2][i] = {1};
}

%Parede de baixo
for(int i = 0; i <= 200; i++){
   Mapa200[199][i] = {1};
   Mapa200[198][i] = {1};
   Mapa200[197][i] = {1};
}*/

float pose[3][1];
float pose_f[3][1];
float PointDetected[2][1];

//posicao inicial
float xa = 0;
float ya = 150;
float rPos_a={{xa}, {ya}};
float rTheta_a = 0;

//posicao final
float xf = 50;
float yf = 150;
float rTheta_f=0;
float rPos_a={{xf}, {yf}};

//Waypoints
int waypoints = 1;
float Wx[1] = {25};
float Wy[1] = {125};

//Inicialização do robô
Robot Robot;
robot(Robot, rPos, rTheta, nW, wPos, wTheta, wRad, wWid, platRad);

//Calculo pose inicial 
getPose(Robot, pose);

//------------VFF------------
int fcr = 460; //Intensidade da força repulsiva 
int fca = 20;  //Intensidade da força atrativa
int KS = 25; 
int KV = 3; 
int KI = 0.05;
float erro_int = 0;
float erro = 0;

float vRobot=0; 
float wRobot=0;

float w = 0;

float phi = 0;

float dt = 0;
float delta_fx = 0;
float delta_fy = 0;

//Força Repulsiva
float fax = 0;
float fay = 0;

//Força Resultante
float frx = 0;
float fry = 0;
float Fx = 0;
float Fy = 0;

float i_aux = 0;
float j_aux = 0;
float dij = 0;
float F = 0;
float angulo = 0;

float x_prox = 0;
float y_prox = 0;

float delta_theta = 0;

/*// 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.
        }

        wait(0.01); 
*/

int main(){
    struct RPLidarMeasurement data;
    
    pc.baud(115200);
    init_communication(&pc);

    // Lidar initialization
    rplidar_motor.period(0.0005f);
    rplidar_motor.write(0.5f);
    lidar.begin(se_lidar);
    lidar.setAngle(0,360);
    setSpeeds(50,50);

    pc.printf("Program started.\n");

    lidar.startThreadScan();

    //Scan for 3s the environment 
    while (T < 3){
        start = clock();
        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.
            ComputeLidarPoint(pose, PointDetected, data.distance, data.angle, float theta)
            occupancy_grid_mapping(log_mapa, Mapa40, d, pose[2], xa, ya, theta, X_grelha, Y_grelha);
        }
        end = clock();
        T = ((double) (end - start)) / CLOCKS_PER_SEC;
    }

    while(1) {
        for(int way = 0; way <= waypoints+1; way++){
            pc.printf("Waypoint %d\n", way);

            if (way < waypoints+1){
                //Vai adicionando os waypoints como pontos finais
                Robot_f = robot([{Wx[way]}, {Wy[way]}],rTheta_f,nW,wPos,wTheta,wRad,wWid,platRad);
                getPose(Robot,pose_f);
                df = sqrt((pose_f[1]-pose[1])^2 + (pose_f[2]-pose[2])^2);
            } 
            elseif (way == waypoints+1){
                //Vai adicionar o ponto final proposto pelo o utilizador
                Robot_f = robot([{rPos_f[1]}, {rPos_f[2]}],rTheta_f,nW,wPos,wTheta,wRad,wWid,platRad);
                getPose(Robot,pose_f);
                df = sqrt((pose_f[1]-pose[1])^2 + (pose_f[2]-pose[2])^2);
            }

            while (df > 5){
                start = clock();
                velRobot2velWheels(vRobot, wRobot, wheelsRadius, wheelsDistance, w);
                nextPose(T, w, wRobot, pose, wheelsRadius, wheelsDistance, pose);

                //FAZER CALCULO DO ON DEMAND
        

                //Força Atrativa
                delta_fx=pose_f[0]-pose[0];
                delta_fy=pose_f[1]-pose[1];

                df=sqrt(delta_fx^2 + delta_fy^2);

                fax=fca*(delta_fx)/df;
                fay=fca*(delta_fy)/df;

                //Força Repulsiva
                frx=0; 
                fry=0;

                for(int j = xa-40; j <= xa+40; j++){
                    for(int i = ya-40; i <= ya+40; i++){
                        if((i > 0) && (i <= Y) && (j > 0) && (j <= X)){
                            if(Mapa40[i][j] ~= 0){
                        //Passar para coordenas cartesianas
                                i_aux=(i-1)*5+2.5;
                                j_aux=(j-1)*5+2.5;

                                dij=sqrt(((i_aux-pose[0])^2+(j_aux-pose[1])^2));

                                F=(fcr*Mapa40(i,j)/(dij^2));
                                angulo=atan2(j_aux-pose[1],i_aux-pose[0]);

                                frx=frx+F*cos(angulo);
                                fry=fry+F*sin(angulo);

                            }
                        }
                    }
                }

                //Força Resultante
                Fx=fax-frx;
                Fy=fay-fry;

                //Posição seguinte
                x_prox=pose[0]+Fx;
                y_prox=pose[1]+Fy;

                //Calculo do Erro
                erro=sqrt((x_prox-pose[0])^2 + (y_prox-pose[1])^2) - 2;
                erro_int=erro_int + erro;

                //Calculo Velocidade  
                vRobot=KV*erro+KI*erro_int;    

                if (vRobot > 50){
                    vRobot = 50;
                }

                //Calculo Phi
                phi=atan2(Fy,Fx);
                phi=atan2(sin(phi),cos(phi));

                delta_theta=atan2(sin(phi-pose[2]),cos(phi-pose[2]));

                //Velocidade Angular
                wRobot=KS*delta_theta;

                //Colocar as velocidades no Robot
                setPose(Robot, pose);

                //Calcula o tempo que recorreu
                end = clock();
                T = ((double) (end - start)) / CLOCKS_PER_SEC;
            }
        }
        pc.printf("Chegou ao ponto final!\n");
    }
}