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


    int *x_cel;
    int *y_cel;
    int x_c;
    int y_c;
    
    int next_x;
    int next_y;
    float auxx;
    float auxy;
    int dist_odometria;
    int x_end;
    int y_end;
    
    //Funcao para criar um mapa simples com um quadrado no meio
void read_map(int Map_Matrix[80][80]){
    for(int i = 0; i < 80; i++){
        for(int j = 0; j < 80; j++){
            if(i == 0 || j==0 || i==79 || j==79){
                Map_Matrix[i][j] = 1;  
            }     
            else{
                Map_Matrix[i][j] = 0;
                if((i >= 2 && j>=2 && i<=10 && j<=10) || (i >=69 && j>=69 && i<=77 && j<=77) || (i >=69 && j>=69 && i<=10 && j<=10) || (i >= 2 && j>=2 && i<=77 && j<=77) || (i >= 2 && j>=2 && i<=10 && j<=10)){
                    Map_Matrix[i][j] = 1;
                }
            }              
        }
    } 
}

/*    
void distancia_prevista (int x , int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80],float dist_odometria){
    
    // x_celula --> posição odometria x
    // y_celula --> posição odometria y
    // Distance --> Leitura do lidar
    
    int find = 0;
    next_x = x;
    next_y = y;
    
    x_c = x_celula;
    y_c = y_celula;
    
    //:::::::::::: calcular estimativa ::::::::::::::::::
    while(find == 0)
     
        if(Map_Matrix[x_c][y_c] == 1){
            find = 1;
            }
        else
            next_x = next_x + 5*cos(Angle); // meti ele a andar para a frente de 5 em 5 cm 
            next_y = next_y + 5*sin(Angle); // isto pode causar algum erro
        
            auxx=next_x/5;
            auxy= next_y/5;
            x_c = ceil(auxx)-1;
            y_c = ceil(auxy) - 1;
        
        
          dist_odometria = ((next_x-x)*(next_x-x)) + ((next_y-y)*(next_y-y));
          dist_odometria = sqrt(dist_odometria);
    
    //return(dist_odometria);
    }// fim do while 
    
    
    // usei o next_x_cel com erro de que ele faz de 5 em 5 cm
    // mas tb posso meter de 1 em 1 ou ate menos

*/
/*
int DistanciaPrevBresh(int x, int y, int x_celula, int y_celula, float angle, int 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;
    
    
int find = 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(find == 0){
        int i = 0;
        if(Map_Matrix[x_cel[i]][y_cel[i]] == 1){
            find = 1;
            }
        else{
           i=i+1;
            }
    }
        
    dist_odometria = ((x_c-x_celula)*(x_c-x_celula)) + ((y_c-y_celula)*(y_c-y_celula));
    dist_odometria = sqrt(dist_odometria);
              
    return(dist_odometria);
    }
    
*/