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