Gonçalo Lopes
/
3
VFH com Lidar
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; }