VFH com Lidar

Dependencies:   BufferedSerial

y_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_y[80];        // Array que Contem as Coordenadas y das Células
int Count_y;            // Contador (de Células)
int m_new_y;            // Declive
int slope_error_new_y;  // Erro Assossiado ao Declive da Reta

int *y_bresenham(int x1, int y1, int x2, int y2){ 
   
    for(int i=0; i<=79; i++){
        Cells_y[i] = 0;
    }
   
    m_new_y = 2 * (y2 - y1); 
   
    slope_error_new_y = m_new_y - (x2 - x1); 

    Count_y = -1;
   
    for (int x = x1; x <= x2; x++){ 
        
        Count_y = Count_y + 1;
        
        Cells_y[Count_y] = y1;
  
        slope_error_new_y += m_new_y; 
  
        if (slope_error_new_y >= 0) 
        { 
            y1++; 
            slope_error_new_y  -= 2 * (x2 - x1); 
        } 
    
    } 
   
    Count_y = Count_y + 1;
    Cells_y[79] = Count_y;
    
    return Cells_y;
}