File content as of revision 8:ad8766cf2ec0:

// 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);

float MapaLog[40][40] = {0};
float Mapa40[40][40];

int main()
{
    //printf("Inicio\n\r");
    pc.baud(115200);
    init_communication(&pc);
    
    pc.printf("======================\n\r");
    pc.printf("Inicio\n\r");
    
    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);
    
    int pose[3] = {20,20}; // Ponto Inicial
    float p_angulo = 0;
    int LidarP[2]; // pontos na plataforma
    int LidarW[2]; // pontos no mundo
    
    /*pc.printf("Inicializacao MapaLog\n\r");
    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(p_angulo), -sin(p_angulo), pose[0]},
                        {sin(p_angulo), cos(p_angulo), 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("Entrar no ciclo\n\r");
    while(leituras < 500){
        if(lidar.pollSensorData(&data) == 0)
        {            
            /*if (UserButton == 0) { // Button is pressed
                break;
            }*/
            
            pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
            
            float radians = (data.angle * static_cast<float>(PI))/180.0f;
            
            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]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance);
            
            leituras++;
        }
    }
    
    // Converter o logaritmo para o mapa 40
    
    rplidar_motor.write(0.0f);
    for(int i=0; i<40; i++){
        for(int j=0; j<40; j++){
            pc.printf("%f", Mapa40[i][j]);
            //send_map(Mapa40[j][i]); // envia linha em linha (j i)
            //send_odometry(1, 2, Mapa40[j][i], 4, 10,10, 30); // faz prints estranhos no Putty
        }
    }
}