From 548e7a5ef5326a4f677f0bbba8d58bfdd326fbb0 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Fri, 10 Nov 2023 14:57:16 +0800 Subject: [PATCH] Mapping Functions (Untested) --- frtos/config/car_config.h | 12 ++ frtos/config/magnetometer_config.h | 22 ++-- frtos/magnetometer/magnetometer_direction.h | 72 ++-------- frtos/magnetometer/map.h | 138 ++++++++++---------- frtos/motor/motor_direction.h | 11 +- 5 files changed, 112 insertions(+), 143 deletions(-) diff --git a/frtos/config/car_config.h b/frtos/config/car_config.h index 98bc088..06ed61d 100644 --- a/frtos/config/car_config.h +++ b/frtos/config/car_config.h @@ -15,6 +15,17 @@ typedef struct s_obs_struct } 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 { obs_t *obs; @@ -22,6 +33,7 @@ typedef struct motor_t *p_right_motor; motor_pid_t *p_pid; direction_t *p_direction; + grid_t *p_grid; } car_struct_t; diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 8da5c41..5dbf481 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,13 +7,11 @@ #define DIRECTION_READ_DELAY (200) -#define NUM_READINGS ( 10 ) // Number of readings to - // take before - // calculating - // direction - -// #define ALPHA ( 0.1f ) // Low Pass Filter -// Coefficient +#define NUM_READINGS \ + (10) // Number of readings to + // take before + // calculating + // direction // LSM303DLHC temperature compensation coefficients #define SCALE_Z (1.0f) // Scale for Z-axis @@ -50,10 +48,12 @@ typedef enum */ typedef enum { - UP = 0, - DOWN = 1, - LEFT = 2, - RIGHT = 3 + FORWARD = 0, + UP = 0, + BACKWARD = 1, + DOWN = 1, + LEFT = 2, + RIGHT = 3 } angle_t; /** diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index bf5e1dc..0c1a0b5 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -80,7 +80,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; @@ -190,11 +191,11 @@ calculate_compass_direction(float yaw) * @param compass_direction Compass Direction */ static inline void -update_orientation_data(float roll, - float pitch, - float yaw, - compass_direction_t compass_direction, - volatile direction_t *g_direction) +update_orientation_data(float roll, + float pitch, + float yaw, + compass_direction_t compass_direction, + volatile direction_t *g_direction) { g_direction->roll = roll; g_direction->roll_angle = (roll > 0) ? LEFT : RIGHT; @@ -211,8 +212,8 @@ update_orientation_data(float roll, * @param magnetometer Magnetometer Data */ static void -read_direction(int16_t acceleration[3], - int16_t magnetometer[3], +read_direction(int16_t acceleration[3], + int16_t magnetometer[3], volatile direction_t *g_direction) { @@ -301,7 +302,7 @@ print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) } void -updateDirection(volatile direction_t * g_direction) +updateDirection(volatile direction_t *g_direction) { int16_t magnetometer[3]; int16_t accelerometer[3]; @@ -317,62 +318,13 @@ updateDirection(volatile direction_t * g_direction) read_direction(accelerometer, magnetometer, 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 monitor_direction_task(void *pvParameters) { volatile direction_t *p_direction = NULL; - p_direction = (direction_t *) pvParameters; + p_direction = (direction_t *)pvParameters; for (;;) { @@ -388,7 +340,7 @@ magnetometer_tasks_init(car_struct_t *car_struct) xTaskCreate(monitor_direction_task, "Direction Task", configMINIMAL_STACK_SIZE, - (void *) car_struct->p_direction, + (void *)car_struct->p_direction, PRIO, &h_direction_task); } diff --git a/frtos/magnetometer/map.h b/frtos/magnetometer/map.h index 2737233..5bc09db 100644 --- a/frtos/magnetometer/map.h +++ b/frtos/magnetometer/map.h @@ -9,120 +9,116 @@ #ifndef 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 -Grid *create_grid(int rows, int cols) { - Grid *grid = (Grid *) malloc(sizeof(Grid)); - grid->rows = rows; - grid->cols = cols; +void +map_init(int rows, int cols, grid_t *p_grid, int initial_x, int initial_y) +{ + 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 - grid->data = (bool **) malloc(rows * sizeof(bool *)); - for (int i = 0; i < rows; i++) { - grid->data[i] = (bool *) malloc(cols * sizeof(bool)); - for (int j = 0; j < cols; j++) { - grid->data[i][j] = false; // Initialize to 'false' (unvisited) + // Allocate memory for the grid + p_grid->data = (bool **)malloc(rows * sizeof(bool *)); + for (int i = 0; i < rows; i++) + { + p_grid->data[i] = (bool *)malloc(cols * sizeof(bool)); + 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 -void mark_cell(Grid *grid, int row, int col) { - if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) { +void +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; } } // 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_t *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 + 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_t *grid) +{ + for (int i = 0; i < grid->rows; i++) + { free(grid->data[i]); } free(grid->data); free(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) { - // Define offsets for different orientations +// Function to update the map based on the car's current orientation and +// position +void +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_y = 0; - switch (orientation) { - case NORTH: - offset_y = 1; - break; - case EAST: - offset_x = 1; - break; - case SOUTH: - offset_y = -1; - break; - case WEST: + // Update the current position based on the forward direction + switch (direction) + { + case DIRECTION_LEFT: offset_x = -1; break; - case NORTH_EAST: + case DIRECTION_RIGHT: offset_x = 1; + break; + case DIRECTION_FORWARD: offset_y = 1; break; - case SOUTH_EAST: - offset_x = 1; + case DIRECTION_BACKWARD: offset_y = -1; 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 + 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 -void print_map() { +void +print_map(grid_t *car_path_grid) +{ // 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 "); -// break; -// case true: -// printf("1 "); -// break; -// } } printf("\n"); } } - -#endif //TEST_PROJECT_MAP_H +#endif // TEST_PROJECT_MAP_H diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 00ad331..e0c7280 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -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 target_yaw The target yaw to spin to * @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; + float current_distance = pp_car_struct->p_right_motor->speed.distance_cm; + set_wheel_direction(direction); 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; vTaskDelay(pdMS_TO_TICKS(50)); }