File content as of revision 11:58187c7de709:

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

void bresenham(float poseX, float poseY, float xf, float yf, float z);

int main()
{
    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;
    
    //Inicializar Mapa das probabilidades a 0.5
    for(int i=0; i<40; i++)
        for(int j=0; j<40; j++)
            Mapa40[i][j]=0.5;
    
    // 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
    
    /*pc.printf("Inicializacao MapaLog\n\r");
    for(int i = 0; i < 40; i++){
        for(int j = 0; j < 40; j++){
            MapaLog[i][j] = 0;
        }
    }*/
    
    
    // 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}};
    float dist;
    
    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 < 1000){
        if(lidar.pollSensorData(&data) == 0)
        {           
            //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;
            
            dist = static_cast<float>(data.distance/10.0f); //converte de mm para cm
            
            LidarP[0] = -dist*sin(radians)- 2.8f;
            LidarP[1] = -dist*cos(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];
            
            /*if (data.angle > 0 && data.angle < 5){
                pc.printf("Pose[0]: %f Pose[1]: %f Dist: %f Ang: %f LidarP[0]: %f LidarP[1]: %f LidarW[0]: %f LidarW[1]: %f\n\r", pose[0], pose[1], data.distance, data.angle, LidarP[0], LidarP[1], LidarW[0], LidarW[1]);
            }*/
            // pontos onde o feixe passou
            bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, dist/5);
            
            leituras++;
        }
        wait(0.01);
    }
    
    // Converter o logaritmo para o mapa 40
    
    pc.printf("\nFIM DO CICLO\n========================\n\n\r");
    
    rplidar_motor.write(0.0f);
    for(int i=0; i<40; i++){
        for(int j=0; j<40; j++){
            //pc.printf("%0.3f ", Mapa40[i][j]);
            send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
            wait(0.1);
        }
        //pc.printf("\n\r");
        //pc.printf("\n-----------------------------\n\r");
    }
}


void bresenham(float poseX, float poseY, float xf, float yf, float z){
    int T, E, A, B;
    int x = static_cast<int>(poseX);
    int y = static_cast<int>(poseY);
    int dx = static_cast<int>(abs(xf - poseX));
    int dy = static_cast<int>(abs(yf - poseY));
    
    int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
    int s2 = static_cast<int>((yf - poseY)/dy);
    
    int interchange = 0;
    
    if (dy > dx){
        T = dx;
        dx = dy;
        dy = T;
        interchange = 1;
    }
    
    E = 2*dy - dx;
    A = 2*dy;
    B = 2*dy - 2*dx;
    
    for (int i = 0; i<dx; i++){
        if (E < 0){
            if (interchange == 1){
                y = y + s2;
            }
            else{
                x = x + s1;
            }
            E = E + A;
        }
        
        else{
            y = y + s2;
            x = x + s1;
            E = E + B;
        }
        
        if (x >= 0 && y >= 0 && x < 40 && y < 40){
            // Mapear mapa do Logaritmo
            MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
            //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
            Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
        }
        
        
                  
    }
}