VFH com Lidar

Dependencies:   BufferedSerial

x_bresenham.cpp

Committer:
xaficz
Date:
2021-05-24
Revision:
7:5fa6f21eb739
Parent:
4:53ac11e9e8b9

File content as of revision 7:5fa6f21eb739:

#include "mbed.h"
#include "BufferedSerial.h"
#include "Robot.h"
#include "Communication.h"
#include "math.h"
#include "rplidar.h"

#include <stdio.h>
#include <stdlib.h>

int Cells_x[80];        // Array que Contem as Coordenadas x das Células
int Count_x;            // Contador (de Células)
int m_new_x;            // Declive
int slope_error_new_x;  // Erro Assossiado ao Declive da Reta

int *x_bresenham(int x1, int y1, int x2, int y2){ 
   
    for(int i=0; i<=79; i++){
        Cells_x[i] = 0;
    }
   
    m_new_x = 2 * (y2 - y1); 
   
    slope_error_new_x = m_new_x - (x2 - x1); 

    Count_x = -1;
   
    for (int x = x1; x <= x2; x++){ 
        
        Count_x = Count_x + 1;
        
        Cells_x[Count_x] = x;
  
        slope_error_new_x += m_new_x; 
  
        if (slope_error_new_x >= 0) 
        { 
            y1++; 
            slope_error_new_x  -= 2 * (x2 - x1); 
        } 
    
    } 
   
    Count_x = Count_x + 1;
    Cells_x[79] = Count_x;
    
    return Cells_x;
}