File content as of revision 7:5fa6f21eb739:

#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);
    RPLidar lidar;
    BufferedSerial se_lidar(PA_9, PA_10);
    PwmOut rplidar_motor(D3);
    
    //DigitalIn Button_VFH(USER_BUTTON);
    
    
//=======================================
        //Para o matlab
        
    //init_communication(Serial *serial_in)
        
//=======================================

    //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 = 45;                      //velocidade da roda esquerda    
    double V_right = 52;                     //velocidade da roda direita 

    
    
    
int main() {
    
    struct RPLidarMeasurement data;
    
    
    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);
    
    //Auxiliares
    int Ciclos = 0;
    int Leituras = 0;
    int AUX = 1;
    int i = 0;
    int fin = 0;
    int x_end;
    int y_end;
    int *x_cel;
    int *y_cel;
    float auxx;
    float auxy;
    
    int 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(ciclos< 10){
        
        //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;


            x = x + (var_d)*cos(phi + var_phi/2);
            y = y + (var_d)*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 = %d, y_Celula = %d\n", x_Celula, y_Celula);
    
    //===========================================
        //Para o matlab
        
        //send_odometry(value1,value2,ticks_left,ticks_right,x,y,theta)
    
    //============================================
    
        if(lidar.pollSensorData(&data)== 0) {
            
                    //Distancia Lida
                    Distance = (data.distance)/10; // aqui este 10 é para cm TENHO QUE VER     
                    
                    
                    //Angulo da leitura
                    Angle = pi*(data.angle)/180;
                    Angle = atan2(sin(Angle),cos(Angle));   //em rad
                    
            pc.printf("Distancia = %f,-----------, Angle= %f\n", Distance,Angle);
    
                    //Distancia suposta
                   // dist_odometria = distancia_prevista(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
                   // dist_odometria = DistanciaPrevBresh(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
                   
                   
    if(Angle < pi/2 && Angle > 3*pi/2) {
    x_end = 80;}
    else{
    x_end = 0;}
    
    if(Angle < 0 && Angle < pi) {
    y_end = 80;}
    else{
    y_end = 0;}
    
    x_cel = x_bresenham(x_Celula, y_Celula, x_end, y_end);
    y_cel = y_bresenham(x_Celula, y_Celula, x_end, y_end);
    
    
    while(fin == 0){
        if(Map_Matrix[x_cel[i]][y_cel[i]] == 1){
            fin = 1;
            }
        else{
           i=i+1;
            }
    }
    
    auxx = ((x_cel[i]-x_Celula)*(x_cel[i]-x_Celula));
    auxy = ((y_cel[i]-y_Celula)*(y_cel[i]-y_Celula));
    dist_odometria = sqrt( auxx + auxy );
    
    pc.printf("DistanciaPREVISTA = %f\n", dist_odometria);
    
 }   
                    
    //==========================================
        //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
                }
                
        // reset de ciclos       
        Ciclos = 0;    
            } // fim do numero de leituras
            
            
        } //fim de if leituras
    //======================================
        
        
         return(0);
    } // fim do while
    
    //return(0);
    
//}