devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Mapping Functions (Untested)

This commit is contained in:
Devoalda 2023-11-10 14:57:16 +08:00
parent c4908eb6f2
commit 548e7a5ef5
5 changed files with 112 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
frtos/config/car_config.h
View File

@ -15,6 +15,17 @@ typedef struct s_obs_struct
} obs_t; } obs_t;
// Define the Map structure
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
int initial_x;
int initial_y;
float * distance_array;
int distance_array_size;
} grid_t;
typedef struct typedef struct
{ {
obs_t *obs; obs_t *obs;
@ -22,6 +33,7 @@ typedef struct
motor_t *p_right_motor; motor_t *p_right_motor;
motor_pid_t *p_pid; motor_pid_t *p_pid;
direction_t *p_direction; direction_t *p_direction;
grid_t *p_grid;
} car_struct_t; } car_struct_t;

22
frtos/config/magnetometer_config.h
View File

@ -7,13 +7,11 @@
#define DIRECTION_READ_DELAY (200) #define DIRECTION_READ_DELAY (200)
#define NUM_READINGS ( 10 ) // Number of readings to #define NUM_READINGS \
// take before (10) // Number of readings to
// calculating // take before
// direction // calculating
// direction
// #define ALPHA ( 0.1f ) // Low Pass Filter
// Coefficient
// LSM303DLHC temperature compensation coefficients // LSM303DLHC temperature compensation coefficients
#define SCALE_Z (1.0f) // Scale for Z-axis #define SCALE_Z (1.0f) // Scale for Z-axis
@ -50,10 +48,12 @@ typedef enum
*/ */
typedef enum typedef enum
{ {
UP = 0, FORWARD = 0,
DOWN = 1, UP = 0,
LEFT = 2, BACKWARD = 1,
RIGHT = 3 DOWN = 1,
LEFT = 2,
RIGHT = 3
} angle_t; } angle_t;
/** /**

72
frtos/magnetometer/magnetometer_direction.h
View File

@ -80,7 +80,8 @@ calculate_yaw_magnetometer(int16_t magnetometer[3])
* @return Compensated Yaw * @return Compensated Yaw
*/ */
float float
compensate_magnetometer(float yaw_mag, int16_t temperature) { compensate_magnetometer(float yaw_mag, int16_t temperature)
{
// Calculate temperature difference from the reference temperature // Calculate temperature difference from the reference temperature
uint delta_temp = temperature - TEMPERATURE_OFFSET; uint delta_temp = temperature - TEMPERATURE_OFFSET;
@ -190,11 +191,11 @@ calculate_compass_direction(float yaw)
* @param compass_direction Compass Direction * @param compass_direction Compass Direction
*/ */
static inline void static inline void
update_orientation_data(float roll, update_orientation_data(float roll,
float pitch, float pitch,
float yaw, float yaw,
compass_direction_t compass_direction, compass_direction_t compass_direction,
volatile direction_t *g_direction) volatile direction_t *g_direction)
{ {
g_direction->roll = roll; g_direction->roll = roll;
g_direction->roll_angle = (roll > 0) ? LEFT : RIGHT; g_direction->roll_angle = (roll > 0) ? LEFT : RIGHT;
@ -211,8 +212,8 @@ update_orientation_data(float roll,
* @param magnetometer Magnetometer Data * @param magnetometer Magnetometer Data
*/ */
static void static void
read_direction(int16_t acceleration[3], read_direction(int16_t acceleration[3],
int16_t magnetometer[3], int16_t magnetometer[3],
volatile direction_t *g_direction) volatile direction_t *g_direction)
{ {
@ -301,7 +302,7 @@ print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle)
} }
void void
updateDirection(volatile direction_t * g_direction) updateDirection(volatile direction_t *g_direction)
{ {
int16_t magnetometer[3]; int16_t magnetometer[3];
int16_t accelerometer[3]; int16_t accelerometer[3];
@ -317,62 +318,13 @@ updateDirection(volatile direction_t * g_direction)
read_direction(accelerometer, magnetometer, g_direction); read_direction(accelerometer, magnetometer, g_direction);
print_orientation_data(*g_direction); print_orientation_data(*g_direction);
// Temperature in degrees Celsius
// printf("Temperature: %d\n", temperature[0]);
// print_orientation_data();
// printf("Direction: ");
// print_direction(g_direction.orientation);
switch (g_direction->orientation)
{
case NORTH:
cur_y++;
break;
case EAST:
cur_x++;
break;
case SOUTH:
cur_y--;
break;
case WEST:
cur_x--;
break;
case NORTH_EAST:
cur_x++;
cur_y++;
break;
case SOUTH_EAST:
cur_x++;
cur_y--;
break;
case SOUTH_WEST:
cur_x--;
cur_y--;
break;
case NORTH_WEST:
cur_x--;
cur_y++;
break;
}
// Update the map based on the direction of the car (N, E, S, W)
// update_map(g_direction.orientation, cur_x, cur_y);
// printf("Current Position: (%d, %d)\n", cur_x, cur_y);
// print_map();
// print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle);
} }
void void
monitor_direction_task(void *pvParameters) monitor_direction_task(void *pvParameters)
{ {
volatile direction_t *p_direction = NULL; volatile direction_t *p_direction = NULL;
p_direction = (direction_t *) pvParameters; p_direction = (direction_t *)pvParameters;
for (;;) for (;;)
{ {
@ -388,7 +340,7 @@ magnetometer_tasks_init(car_struct_t *car_struct)
xTaskCreate(monitor_direction_task, xTaskCreate(monitor_direction_task,
"Direction Task", "Direction Task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *) car_struct->p_direction, (void *)car_struct->p_direction,
PRIO, PRIO,
&h_direction_task); &h_direction_task);
} }

138
frtos/magnetometer/map.h
View File

@ -9,120 +9,116 @@
#ifndef TEST_PROJECT_MAP_H #ifndef TEST_PROJECT_MAP_H
#define TEST_PROJECT_MAP_H #define TEST_PROJECT_MAP_H
// Define the grid structure
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
} Grid;
// Global grid to track the car's path
Grid *car_path_grid;
// Function to create and initialize a grid // Function to create and initialize a grid
Grid *create_grid(int rows, int cols) { void
Grid *grid = (Grid *) malloc(sizeof(Grid)); map_init(int rows, int cols, grid_t *p_grid, int initial_x, int initial_y)
grid->rows = rows; {
grid->cols = cols; p_grid->rows = rows;
p_grid->cols = cols;
p_grid->initial_x = initial_x;
p_grid->initial_y = initial_y;
// Allocate memory for the 2D array // Allocate memory for the grid
grid->data = (bool **) malloc(rows * sizeof(bool *)); p_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++) { p_grid->data[i] = (bool *)malloc(cols * sizeof(bool));
grid->data[i][j] = false; // Initialize to 'false' (unvisited) for (int j = 0; j < cols; j++)
{
p_grid->data[i][j] = false; // Initialize to 'false' (unvisited)
} }
} }
return grid;
} }
// Function to mark a cell as visited // Function to mark a cell as visited
void mark_cell(Grid *grid, int row, int col) { void
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) { mark_cell(grid_t *grid, int row, int col)
{
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
{
grid->data[row][col] = true; grid->data[row][col] = true;
} }
} }
// Function to check if a cell has been visited // Function to check if a cell has been visited
bool is_cell_visited(Grid *grid, int row, int col) { bool
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) { is_cell_visited(grid_t *grid, int row, int col)
{
if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
{
return grid->data[row][col]; return grid->data[row][col];
} }
return false; // Consider out-of-bounds as unvisited return false; // Consider out-of-bounds as unvisited
} }
// Function to destroy the grid and free memory // Function to destroy the grid and free memory
void destroy_grid(Grid *grid) { void
for (int i = 0; i < grid->rows; i++) { destroy_grid(grid_t *grid)
{
for (int i = 0; i < grid->rows; i++)
{
free(grid->data[i]); free(grid->data[i]);
} }
free(grid->data); free(grid->data);
free(grid); free(grid);
} }
// Function to update the map based on car's current orientation // Function to update the map based on the car's current orientation and
// Function to update the map based on car's current orientation and position // position
void update_map(int orientation, int cur_x, int cur_y) { void
// Define offsets for different orientations update_map(grid_t *car_path_grid, uint32_t direction, float distance)
{
int cur_x = car_path_grid->initial_x;
int cur_y = car_path_grid->initial_y;
// Define offsets for different forward and turn directions
int offset_x = 0; int offset_x = 0;
int offset_y = 0; int offset_y = 0;
switch (orientation) { // Update the current position based on the forward direction
case NORTH: switch (direction)
offset_y = 1; {
break; case DIRECTION_LEFT:
case EAST:
offset_x = 1;
break;
case SOUTH:
offset_y = -1;
break;
case WEST:
offset_x = -1; offset_x = -1;
break; break;
case NORTH_EAST: case DIRECTION_RIGHT:
offset_x = 1; offset_x = 1;
break;
case DIRECTION_FORWARD:
offset_y = 1; offset_y = 1;
break; break;
case SOUTH_EAST: case DIRECTION_BACKWARD:
offset_x = 1;
offset_y = -1; offset_y = -1;
break; break;
case SOUTH_WEST:
offset_x = -1;
offset_y = -1;
break;
case NORTH_WEST:
offset_x = -1;
offset_y = 1;
break;
} }
// Update the map based on the car's current position and orientation cur_x += offset_x;
cur_y += offset_y;
// Mark the current and next position as visited
mark_cell(car_path_grid, cur_x, cur_y); mark_cell(car_path_grid, cur_x, cur_y);
mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y); car_path_grid->initial_x = cur_x;
car_path_grid->initial_y = cur_y;
car_path_grid->distance_array[car_path_grid->distance_array_size]
= distance;
car_path_grid->distance_array_size++;
} }
// Function to print the map // Function to print the map
void print_map() { void
print_map(grid_t *car_path_grid)
{
// Invert the map, 0,0 is at the Middle // Invert the map, 0,0 is at the Middle
// Print 1 for visited cells and 0 for unvisited cells // Print 1 for visited cells and 0 for unvisited cells
for (int i = car_path_grid->rows - 1; i >= 0; i--) { for (int i = car_path_grid->rows - 1; i >= 0; i--)
for (int j = 0; j < car_path_grid->cols; j++) { {
for (int j = 0; j < car_path_grid->cols; j++)
{
(car_path_grid->data[j][i]) ? printf("1 ") : printf("0 "); (car_path_grid->data[j][i]) ? printf("1 ") : printf("0 ");
// case false:
// printf("0 ");
// break;
// case true:
// printf("1 ");
// break;
// }
} }
printf("\n"); printf("\n");
} }
} }
#endif // TEST_PROJECT_MAP_H
#endif //TEST_PROJECT_MAP_H

11
frtos/motor/motor_direction.h
View File

@ -70,7 +70,7 @@ check_direction(float current_direction, float target_direction, float range)
} }
/*! /*!
* @brief Spin the car to a certain yaw specifically * @brief Spin the car to a certain yaw specifically, update the map after
* @param direction The direction to turn or spin * @param direction The direction to turn or spin
* @param target_yaw The target yaw to spin to * @param target_yaw The target yaw to spin to
* @param pwm_level The pwm_level of the wheels, from 0 to 99 * @param pwm_level The pwm_level of the wheels, from 0 to 99
@ -84,6 +84,8 @@ turn_to_yaw(uint32_t direction,
{ {
pp_car_struct->p_pid->use_pid = false; pp_car_struct->p_pid->use_pid = false;
float current_distance = pp_car_struct->p_right_motor->speed.distance_cm;
set_wheel_direction(direction); set_wheel_direction(direction);
set_wheel_speed_synced(pwm_level, pp_car_struct); set_wheel_speed_synced(pwm_level, pp_car_struct);
@ -99,6 +101,13 @@ turn_to_yaw(uint32_t direction,
} }
} }
float distance_traveled
= pp_car_struct->p_right_motor->speed.distance_cm - current_distance;
// Update Map after turning
update_map(pp_car_struct->p_grid, direction, distance_traveled);
pp_car_struct->p_pid->use_pid = true; pp_car_struct->p_pid->use_pid = true;
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
} }