File content as of revision 5:bc42c03f2a23:

// 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 "math.h"

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

#define PI 3.1415926535

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

int main()
{
    
    pc.baud(115200);
    init_communication(&pc);
    
    DigitalIn UserButton(USER_BUTTON); // Initialize Button
    DigitalOut myled(LED1); // Initialize LED
    
    //float odomX, odomY, odomTheta;
    struct RPLidarMeasurement data;
    
    // 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
    float LidarP[2]; // pontos na plataforma
    float LidarW[2]; // pontos no mundo
    float MapaLog[40][40];
    for(int i = 0; i < 40; i++)
        for(int j = 0; j < 40; j++)
            MapaLog[i][j] = 0;
    
    float Mapa40[40][40];
    
    // matriz rotacao world plataforma 
    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
                        {sin(pose[2]), cos(pose[2]), pose[1]},
                        {0, 0, 1}};
    
    setSpeeds(0,0);
    
    int leituras = 0;
    
    pc.printf("waiting...\n\r");
    
    int start = 0;
    while(start != 1) {
        myled=1;
        if (UserButton == 0) { // Button is pressed
            myled = 0;
            start = 1;
            rplidar_motor.write(0.5f);
        }
    }
    
    lidar.startThreadScan();
    
    pc.printf("Program started.\n\r");
    
    while(1){
        if(lidar.pollSensorData(&data) == 0)
        {
            if(leituras == 100){
                break;
            }
            pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
            
            float radians = (data.angle * PI)/180.0;
            
            LidarP[0] = -data.distance*cos(radians)- 2.8f;
            LidarP[1] = -data.distance*sin(radians)- 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
            bresenham(pose[0], pose[1], LidarW[0], LidarW[1], MapaLog, data.distance);
            
            leituras++;
        }
    }
    
    // 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 (j i)
        }
    }
       
    /*
    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); 
    }*/
}