File content as of revision 4:256f2cbe3fdd:

// Coded by Luís Afonso 11-04-2019
#include "mbed.h"
#include "BufferedSerial.h"
#include "rplidar.h"
#include "Robot.h"
#include "Communication.h"
#include "Functions.h"

#include <stdlib.h>
#include <stdio.h>

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

int main()
{
    
    float odomX, odomY, odomTheta;
    struct RPLidarMeasurement data;
    
    pc.baud(115200);
    init_communication(&pc);
    pc.printf("Program started.\n\r");
    
    // Lidar initialization
    rplidar_motor.period(0.001f);
    rplidar_motor.write(0.5f);
    lidar.begin(se_lidar);
    lidar.setAngle(0,360);
    
    float pose[3] = {20,20,0}; // Ponto Inicial
    //int** pointsVec; // ponteiro duplo para a tabela dimensional que guarda os valores da funcao bresenham
    float LidarP[2]; // pontos na plataforma
    float LidarW[2]; // pontos no mundo
    float MapaLog[40][40] = {0};
    float Mapa40[40][40];
    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},// matriz rotacao world plataforma 
                        {sin(pose[2]), cos(pose[2]), pose[1]},
                        {0, 0, 1}};
    
    int dim; // guarda a dimensao (numero de linhas) de pointsVec
    
    pc.printf("Program started.\n\r");
    
    //lidar.startThreadScan();
    
    setSpeeds(0,0);
    
    float dist = 10;
    float angle = 0;
    
    //while(1){
        //if(lidar.pollSensorData(&data) == 0)
        //{
            //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
            
            //radians = ( data.angle * pi ) / 180;
            
            //LidarP[0] = -data.distance*cos(radians)- 2.8f;
            //LidarP[1] = -data.distance*sin(radians)- 1.5f;
            LidarP[0] = -dist*cos(angle)- 2.8f;
            LidarP[1] = -dist*sin(angle)- 1.5f;

            //W_P = R_WP * p_P
            LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
            LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
            
            // pontos onde o feixe passou
            pointsVec = bresenham(pose[0], pose[1], LidarW[0], LidarW[1], &dim);
            
            for(int i=0; i<dim; i++){
                pc.printf("%d, %d\n", pointsVec[i][0], pointsVec[i][1]);
            }
            
            
            /* para estes valores o resultado é o seguinte
            dist: 10.000000   angle: 0.000000

            dim: 13
            19, 20
            18, 20
            17, 20
            16, 20
            15, 19
            14, 19
            13, 19
            12, 19
            11, 19
            10, 19
            9, 19
            8, 19
            7, 18*/
            
            // Mapear mapa do Logaritmo
            //Mapping(MapaLog, pose[0], pose[1], pointsVec, data.distance, dim);           
                        
            /* //libertar espaco da variavel pointsVec
            for (int h = 0; h < height; h++){
                delete [] pointsVec[h];
            }
            delete [] pointsVec;
            pointsVec = 0;*/
            
        //}
        
    //}
    
    // Converter o logaritmo para o mapa 40
    
    for(int i=0; i<40; i++){
        for(int j=0; j<40; j++){
            Mapa40[j][i] = 1 - 1/(1+exp(MapaLog[j][i]));
            //printf("%.2f\n", 1 - 1/(1+exp(MapaLog[i][j])));
            send_map(Mapa40[j][i]); // envia linha em linha
        }
    }
       
    /*
    while(1) {
        // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
        if(lidar.pollSensorData(&data) == 0)
        {
            //if (data.angle > 0 and data.angle < 15)
                pc.printf("%f\t%f\t%d\t%c\n\r", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
                send_odometry(1, 2, countsLeft, countsRight, odomX, odomY, odomTheta);
        }
       wait(0.01); 
    }*/
}