File content as of revision 4:213afe3d5c4b:

#include "mbed.h"
#include "BufferedSerial.h"
#include "rplidar.h"
#include "Robot.h"
#include "Communication.h"
#include "funcs.h"

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

static const double pi = 3.14159265358;


//Matrizes de mapeamento
int Map_Matrix[80][80] = {{0}};     //Matriz do mapa
double Log_Map_Matrix[80][80] = {{0}}; //Matriz de Logs
int Aux_Matrix[80][80] = {{0}};
int Sector_Matrix_Sections[15][15]; //Matriz com os indices das seccoes
double Sector_Matrix_Abs[15][15];   //Matriz com as amplitudes 
int Surroundings_Matrix[15][15];    //Matriz das redondezas do bot
double angle;    
    
//Ganhos
double k = 12;                       //ganho angular
    
//Variaveis de posicao
double x = 100;                     //x inicial
double y = 2000;                     //y inicial
double phi = 0.01;                  //phi inicial
double end_position_x = 1900;        //x final
double end_position_y = 2000;       //y final
double next_phi;                    //phi seguinte
    
//Variaveis de estimacao de posicao
double var_l;                       //contagens do encoder esquerdas
double var_r;                       //contagens do encoder direitas
double var_d;                       //variacao linear entre iteracoes
double var_phi;                     //variacao angular entre iteracoes
double L = 150;                     //distancia entre rodas
double r = 35;                      //raio da roda
    
//Variaveis de comando as rodas
double w;                           //velociade angular do robo
double w_left;                      //velocidade roda esquerda
double w_right;                     //velocidade roda direita
double v = 2100;                    //velocidade linear do robo   
double dist;
    
int main()
{
    while (Button == 1) {}
    wait(1);
    
    //float odomX, odomY, odomTheta;
    struct RPLidarMeasurement data;
    
    pc.baud(115200);
    init_communication(&pc);

    // Lidar initialization
    rplidar_motor.write(1.0f);
    rplidar_motor.period(0.001f);
    lidar.begin(se_lidar);
    lidar.setAngle(0,360);

    lidar.startThreadScan();

    //--------------------------------------------------------------------------
    
    //Espera que o botao seja premido
    while (Button == 1) {}
    wait(1);
    
    //Funcao para obter a matriz com os indices das seccoes assim como as amplitudes
    get_Sector_Matrix(Sector_Matrix_Sections, Sector_Matrix_Abs);
    
    //Distancia ao ponto objetivo
    dist = sqrt((double)pow(end_position_x-x,2) + pow(end_position_y-y,2));
    
    while(dist > 50) {
        // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
        if(lidar.pollSensorData(&data) == 0)
        {
            //pc.printf("%f;%f;%d;%c;\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
        
        
        getCountsAndReset();
    
        //Estimacao de pose
        var_l = (2*pi*r*countsLeft/1440);
        var_r = (2*pi*r*countsRight/1440);
        var_d = (var_l+var_r)/2;
        var_phi= (var_r-var_l)/L;

        if(var_phi == 0){
            x = x + var_d*cos(phi);
            y = y + var_d*sin(phi);
        } 
        else{
            x = x + (var_d)*((sin(phi/2)/(phi/2))*cos(phi + var_phi/2));
            y = y + (var_d)*((sin(phi/2)/(phi/2))*sin(phi + var_phi/2));
            phi = phi + var_phi;
        }
        //Print coordenadas habituais
        //pc.printf("x_atual = %f, y_atual = %f\n",x,y);
        //Distancia ao ponto objetivo
        dist = sqrt((double)pow(end_position_x-x,2) + pow(end_position_y-y,2));
        //pc.printf("\ndist = %f\n", dist);
        
        //Conversao dos angulos em radianos
        angle = (2*pi/360)*(data.angle); //
        angle = atan2(sin(angle + pi/2 + phi),cos(angle + pi/2 + phi));
        update_map(Log_Map_Matrix, Aux_Matrix, Map_Matrix, data.distance/50, angle, x, y); //Funcao para atualizar o mapa
        
        //Funcao para obter o mapa das redondezas do robo
        get_Surroundings_Matrix(Surroundings_Matrix, x, y, Map_Matrix);
        
        for(int n = 0; n < 15; n++){
            for(int m = 0; m < 15; m++){
                pc.printf("%d,",Surroundings_Matrix[n][m]);
            }
            pc.printf("\n");
        }
        
        //Funcao para obter a direcao que o robo deve tomar em funcao da sua posicao atual e das suas redondezas    
        next_phi = get_orientation_angle(x, y, end_position_x, end_position_y, Surroundings_Matrix, Sector_Matrix_Sections, Sector_Matrix_Abs, 31);
        //pc.printf("\n\n next phi = %f\n\n", next_phi);
        
        //Controlo proporcional da direcao
        w = k*atan2(sin(next_phi-phi),cos(next_phi-phi));
        
        //Enviar os comandos de velocidades para as rodas
        w_left=(v-(L/2)*w)/r;
        w_right=(v+(L/2)*w)/r;
        setSpeeds(w_left,w_right); 
        }   
    }
    setSpeeds(0,0);
    
    return(0);
    }