refactor(Code Cleanup): Modified code structure
This commit is contained in:
parent
b541cc620f
commit
adaa34b1ae
|
@ -2,7 +2,6 @@
|
|||
#include "line_sensor_init.h"
|
||||
#include "car_config.h"
|
||||
|
||||
|
||||
int
|
||||
main(void)
|
||||
{
|
||||
|
|
|
@ -9,11 +9,15 @@
|
|||
* The yaw is calculated using the magnetometer and accelerometer data
|
||||
* The roll, pitch and yaw are combined to calculate the direction
|
||||
* of the car with a complementary filter and compensating for the
|
||||
* temperature.
|
||||
* temperature. (existing)
|
||||
*
|
||||
* The complementary filter is used to combine the accelerometer
|
||||
* and magnetometer data (yaw) to calculate the direction of the car
|
||||
*
|
||||
* Complementary Filter was used previously, but it was not accurate
|
||||
* enough. The yaw calculated from the magnetometer data was used
|
||||
* instead.
|
||||
*
|
||||
* Source:
|
||||
* https://www.nxp.com/docs/en/application-note/AN3461.pdf
|
||||
* https://ahrs.readthedocs.io/en/latest/filters/complementary.html
|
||||
|
@ -80,7 +84,8 @@ calculate_yaw_magnetometer(int16_t magnetometer[3])
|
|||
* @return Compensated Yaw
|
||||
*/
|
||||
float
|
||||
compensate_magnetometer(float yaw_mag, int16_t temperature) {
|
||||
compensate_magnetometer(float yaw_mag, int16_t temperature)
|
||||
{
|
||||
// Calculate temperature difference from the reference temperature
|
||||
uint delta_temp = temperature - TEMPERATURE_OFFSET;
|
||||
|
||||
|
|
|
@ -9,9 +9,9 @@
|
|||
#ifndef TEST_PROJECT_MAP_H
|
||||
#define TEST_PROJECT_MAP_H
|
||||
|
||||
|
||||
// Define the grid structure
|
||||
typedef struct {
|
||||
typedef struct
|
||||
{
|
||||
bool **data; // 2D array to represent the grid
|
||||
int rows; // Number of rows in the grid
|
||||
int cols; // Number of columns in the grid
|
||||
|
@ -21,16 +21,20 @@ typedef struct {
|
|||
Grid *car_path_grid;
|
||||
|
||||
// Function to create and initialize a grid
|
||||
Grid *create_grid(int rows, int cols) {
|
||||
Grid *
|
||||
create_grid(int rows, int cols)
|
||||
{
|
||||
Grid *grid = (Grid *)malloc(sizeof(Grid));
|
||||
grid->rows = rows;
|
||||
grid->cols = cols;
|
||||
|
||||
// Allocate memory for the 2D array
|
||||
grid->data = (bool **)malloc(rows * sizeof(bool *));
|
||||
for (int i = 0; i < rows; i++) {
|
||||
for (int i = 0; i < rows; i++)
|
||||
{
|
||||
grid->data[i] = (bool *)malloc(cols * sizeof(bool));
|
||||
for (int j = 0; j < cols; j++) {
|
||||
for (int j = 0; j < cols; j++)
|
||||
{
|
||||
grid->data[i][j] = false; // Initialize to 'false' (unvisited)
|
||||
}
|
||||
}
|
||||
|
@ -39,23 +43,32 @@ Grid *create_grid(int rows, int cols) {
|
|||
}
|
||||
|
||||
// Function to mark a cell as visited
|
||||
void mark_cell(Grid *grid, int row, int col) {
|
||||
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
|
||||
void
|
||||
mark_cell(Grid *grid, int row, int col)
|
||||
{
|
||||
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
|
||||
{
|
||||
grid->data[row][col] = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Function to check if a cell has been visited
|
||||
bool is_cell_visited(Grid *grid, int row, int col) {
|
||||
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
|
||||
bool
|
||||
is_cell_visited(Grid *grid, int row, int col)
|
||||
{
|
||||
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
|
||||
{
|
||||
return grid->data[row][col];
|
||||
}
|
||||
return false; // Consider out-of-bounds as unvisited
|
||||
}
|
||||
|
||||
// Function to destroy the grid and free memory
|
||||
void destroy_grid(Grid *grid) {
|
||||
for (int i = 0; i < grid->rows; i++) {
|
||||
void
|
||||
destroy_grid(Grid *grid)
|
||||
{
|
||||
for (int i = 0; i < grid->rows; i++)
|
||||
{
|
||||
free(grid->data[i]);
|
||||
}
|
||||
free(grid->data);
|
||||
|
@ -64,12 +77,15 @@ void destroy_grid(Grid *grid) {
|
|||
|
||||
// Function to update the map based on car's current orientation
|
||||
// Function to update the map based on car's current orientation and position
|
||||
void update_map(int orientation, int cur_x, int cur_y) {
|
||||
void
|
||||
update_map(int orientation, int cur_x, int cur_y)
|
||||
{
|
||||
// Define offsets for different orientations
|
||||
int offset_x = 0;
|
||||
int offset_y = 0;
|
||||
|
||||
switch (orientation) {
|
||||
switch (orientation)
|
||||
{
|
||||
case NORTH:
|
||||
offset_y = 1;
|
||||
break;
|
||||
|
@ -105,13 +121,16 @@ void update_map(int orientation, int cur_x, int cur_y) {
|
|||
mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y);
|
||||
}
|
||||
|
||||
|
||||
// Function to print the map
|
||||
void print_map() {
|
||||
void
|
||||
print_map()
|
||||
{
|
||||
// Invert the map, 0,0 is at the Middle
|
||||
// Print 1 for visited cells and 0 for unvisited cells
|
||||
for (int i = car_path_grid->rows - 1; i >= 0; i--) {
|
||||
for (int j = 0; j < car_path_grid->cols; j++) {
|
||||
for (int i = car_path_grid->rows - 1; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j < car_path_grid->cols; j++)
|
||||
{
|
||||
(car_path_grid->data[j][i]) ? printf("1 ") : printf("0 ");
|
||||
// case false:
|
||||
// printf("0 ");
|
||||
|
|
Loading…
Reference in New Issue