File content as of revision 6:df6b8b2468d8:

#include "mbed.h"
#include "BufferedSerial.h"
#include "rplidar.h"
#include "Robot.h"
#include "Communication.h"
#include <string.h>
#include <stdio.h>
#include <math.h>

#define pi 3.14159265359

    Serial pc(SERIAL_TX, SERIAL_RX);
    DigitalIn Button_VFH(USER_BUTTON);

    RPLidar lidar;
    BufferedSerial se_lidar(PA_9, PA_10);
    PwmOut rplidar_motor(D3);

struct RPLidarMeasurement data;

int main() {
    
    pc.baud(115200);
    init_communication(&pc);

    rplidar_motor.period(0.01f);
    //rplidar_motor.write(1.0f);
    lidar.begin(se_lidar);
    lidar.setAngle(0,360);
    lidar.startThreadScan();
    rplidar_motor.write(0.7f);
    
    
    //Matrizes de mapeamento
    int Map_Matrix[80][80];             //Matriz do mapa
    
    //Ganhos
   // double k = 8;                       //Ganho angular
    
    //Variaveis de posicao
    double x = 60;                     //x inicial cm
    double y = 60;                     //y inicial
    double phi = 0.01;                  //phi inicial
    
    
    // Coordenadas
    int x_Celula;       // Coordenada X Relativa à Celula Ativa (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
    int y_Celula;       // Coordenada Y Relativa à Celula Ativa (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
    
    
    float Distance;         // Distância Devolvida pela Leitura do LIDAR
    float Angle;            // Angulo Devolvida pela Leitura do LIDAR
    
    
    //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 = 15.0;                     //distancia entre rodas 
    double r = 3.5;                      //raio da roda
    
    //Variaveis de comando as rodas
    double V_left = 20;                      //velocidade da roda esquerda    
    double V_right = 25;                     //velocidade da roda direita 

    //Auxiliares
    int Ciclos = 0;
    int Leituras = 0;
    int AUX = 0;
    
    
    double dist_odometria;
    
    //Funcao de obtencao do Mapa    
    read_map(Map_Matrix);              
    
    
  while(1){
        
        //Este while é para fazer uma paragem de 10 em 10 ciclos para dar tempo para o lidar executar leituras
        while(AUX == 1){
        
        //Manda andar 
        setSpeeds(V_left,V_right);
        
        
    //:::::::::::::: ODOMETRIA :::::::::::::::
    
        //Obter as contagens dos encoders
        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;
        }
        
        pc.printf("x_odometria = %f, y_odometria = %f\n", x, y);
        
    
        //Celula da Odometria
            x_Celula = ceil(x/5) - 1;
            y_Celula = ceil(y/5) - 1;
    
        pc.printf("x_Celula = %f, y_Celula = %f\n", x_Celula, y_Celula);
    
    
    //============================================
    
    
    
    
                    //Distancia suposta
                    dist_odometria = distancia_prevista(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
    
    
    
    
    
    
    
    //==========================================
        //conta como mais um ciclo
        Ciclos = Ciclos+1;
        
     
        
    //::::::::::::::IF de LEITURAS ::::::::::::::::::::::::::: 
        if(Ciclos == 10){
            while(Leituras<30) // faz 30 leituras parado
            
                if(lidar.pollSensorData(&data)== 0) {
            
                    Distance = (data.distance)/10; // aqui este 10 é para cm TENHO QUE VER     
                    Angle = pi*(data.angle)/180;
                    Angle = atan2(sin(Angle),cos(Angle));   //em rad
                    
                    //Distancia suposta
                    dist_odometria = distancia_prevista(x, y, x_celula, y_celula, Angle,Map_Matrix[80][80]){ 
            
            
                Leituras = Leituras + 1; // incrementa as leituras
                }
                
                
        Ciclos = 0;    
            } // fim do numero de leituras
            
            
        } //fim de if leituras
    //======================================
        
        
        
    } // fim do while
    
    
    
    

    
    // reset de ciclos
    // 

    
    return(0);
}