From c7510fd63f5025a4c3af20ef750294bf43d32cc1 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Tue, 28 Nov 2023 08:25:10 +0800 Subject: [PATCH] feature(MAP): Created Baseline Mapping Function --- frtos/CMakeLists.txt | 1 + frtos/config/car_config.h | 27 ++- frtos/magnetometer/magnetometer_test.c | 9 - frtos/map/CMakeLists.txt | 20 ++ frtos/map/map_test.c | 24 ++ frtos/map/mapping.h | 316 +++++++++++++++++++++++++ 6 files changed, 387 insertions(+), 10 deletions(-) create mode 100644 frtos/map/CMakeLists.txt create mode 100644 frtos/map/map_test.c create mode 100644 frtos/map/mapping.h diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index 3df3d1e..a9730b9 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -6,6 +6,7 @@ add_subdirectory(car) add_subdirectory(ultrasonic_sensor) add_subdirectory(magnetometer) add_subdirectory(frontend) +add_subdirectory(map) add_executable(rtos_car rtos_car.c) diff --git a/frtos/config/car_config.h b/frtos/config/car_config.h index 13d6ca4..a27d7ad 100644 --- a/frtos/config/car_config.h +++ b/frtos/config/car_config.h @@ -6,7 +6,9 @@ #ifndef CAR_CONFIG_H #define CAR_CONFIG_H -#define PRIO (tskIDLE_PRIORITY + 1UL) +#define PRIO (tskIDLE_PRIORITY + 1UL) +#define MAX_HEIGHT 10 +#define MAX_WIDTH 10 typedef struct s_obs_struct { @@ -16,6 +18,28 @@ typedef struct s_obs_struct } obs_t; +typedef enum e_direction +{ + up, + down, + left, + right +} mapping_direction_t; + +typedef struct +{ + char type; + int reachable; + int visited; +} mazecells_t; + +typedef struct +{ + int height; + int width; + mazecells_t mazecells[MAX_HEIGHT][MAX_WIDTH]; +} maze_t; + typedef struct { obs_t *obs; @@ -23,6 +47,7 @@ typedef struct motor_t *p_right_motor; motor_pid_t *p_pid; direction_t *p_direction; + maze_t *p_maze; } car_struct_t; diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c index 26f29d7..b22b69a 100644 --- a/frtos/magnetometer/magnetometer_test.c +++ b/frtos/magnetometer/magnetometer_test.c @@ -1,7 +1,6 @@ #include "magnetometer_init.h" #include "magnetometer_direction.h" -#include "map.h" #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL) @@ -29,23 +28,15 @@ main(void) car_struct_t car_struct = { .p_direction = &direction }; - int grid_rows = 10; // Define the number of rows in your grid - int grid_cols = 10; // Define the number of columns in your grid - - car_path_grid = create_grid(grid_rows, grid_cols); - sleep_ms(2000); printf("Test started!\n"); magnetometer_init(&car_struct); - // printf("Magnetometer initialized!\n"); - magnetometer_tasks_init(&car_struct); vTaskStartScheduler(); - // launch(); return (0); } \ No newline at end of file diff --git a/frtos/map/CMakeLists.txt b/frtos/map/CMakeLists.txt new file mode 100644 index 0000000..9402536 --- /dev/null +++ b/frtos/map/CMakeLists.txt @@ -0,0 +1,20 @@ +add_executable( + map_test + map_test.c +) + +target_link_libraries( + map_test + hardware_adc + pico_stdlib + pico_rand + FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap +) + +target_include_directories(map_test + PRIVATE ../config + ../car +) + +pico_enable_stdio_usb(map_test 1) +pico_add_extra_outputs(map_test) \ No newline at end of file diff --git a/frtos/map/map_test.c b/frtos/map/map_test.c new file mode 100644 index 0000000..67cbd9f --- /dev/null +++ b/frtos/map/map_test.c @@ -0,0 +1,24 @@ + +#include "mapping.h" +#include "car_config.h" + +int +main(void) +{ + stdio_usb_init(); + maze_t maze; + + sleep_ms(7000); + + printf("Test started!\n"); + + mapping_init(&maze); + printf("Mapping initialized!\n"); + + mapping_tasks_init(&maze); + printf("Mapping tasks initialized!\n"); + + vTaskStartScheduler(); + + return (0); +} \ No newline at end of file diff --git a/frtos/map/mapping.h b/frtos/map/mapping.h new file mode 100644 index 0000000..07ecd9f --- /dev/null +++ b/frtos/map/mapping.h @@ -0,0 +1,316 @@ +/** + * @file mapping.h + * @brief Map the environment using the line sensor and the ultrasonic sensor + * @author Woon Jun Wei + */ + +#ifndef MAPPING_H +#define MAPPING_H + +#include +#include "pico/stdlib.h" +#include "time.h" +#include "pico/rand.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "message_buffer.h" +#include "semphr.h" + +#include "car_config.h" + +// Function to generate a random number between min and max (inclusive) +int +generate_random(int min, int max) +{ + int num = (get_rand_32() % (max - min + 1)) + min; + printf("Random number generated: %d\n", num); + return num; +} + +/** + * Create a map with hardcoded walls, obstacles, and the goal + * With the start point at the bottom left corner. + * Ensures there is at least one clear path from start to goal. + * @param maze + */ +void create_map(maze_t *maze) +{ + // Create the map based on maze height and width + for (int i = 0; i < maze->height; i++) + { + for (int j = 0; j < maze->width; j++) + { + if (i == 0 || i == maze->height - 1 || j == 0 || j == maze->width - 1) + { + maze->mazecells[i][j].type = 'X'; // Walls at the border + } + else + { + // Randomly place walls and obstacles + if (generate_random(0, 9) < 2) // Adjust the threshold for more or fewer obstacles + { + maze->mazecells[i][j].type = 'X'; // Obstacle + } + else + { + maze->mazecells[i][j].type = ' '; // Empty space + } + } + maze->mazecells[i][j].reachable = 0; + maze->mazecells[i][j].visited = 0; + } + } + + // Set the start point + maze->mazecells[0][0].type = 'S'; + maze->mazecells[0][0].reachable = 1; + maze->mazecells[0][0].visited = 1; + + // Set the goal (assuming it's at the top-right corner) + maze->mazecells[maze->height - 1][maze->width - 1].type = 'G'; + + // Ensure there is a clear path from start to goal + for (int i = 1; i < maze->height - 1; i++) + { + maze->mazecells[i][maze->width / 2].type = ' '; // Clear path + } +} + +/** + * Create a hardcoded map with a clear path from start to goal + * @param maze + */ +void create_hardcoded_map(maze_t *maze) +{ + // Set fixed height and width during initialization + maze->height = 5; + maze->width = 5; + + // Create the map with a clear path + char hardcoded_map[5][5] = { + {'S', ' ', ' ', ' ', 'G'}, + {' ', ' ', 'X', ' ', ' '}, + {' ', ' ', ' ', ' ', ' '}, + {' ', 'X', ' ', ' ', ' '}, + {'C', ' ', 'X', ' ', ' '}, + }; + + // Copy the hardcoded map to the maze structure + for (int i = 0; i < maze->height; i++) + { + for (int j = 0; j < maze->width; j++) + { + maze->mazecells[i][j].type = hardcoded_map[i][j]; + maze->mazecells[i][j].reachable = 0; + maze->mazecells[i][j].visited = 0; + } + } +} + +/** + * @brief Mapping Initialization + */ +void +mapping_init(maze_t *p_maze) +{ + printf("Initializing mapping\n"); + + // Set fixed height and width during initialization + p_maze->height = MAX_HEIGHT / 2; + p_maze->width = MAX_WIDTH / 2; + + // Create the maze + printf("Creating maze\n"); + create_hardcoded_map(p_maze); + printf("Maze created\n"); +} + +/** + * @brief Print the map + * @param maze + */ +void print_map(maze_t *maze) +{ + for (int i = maze->height - 1; i >= 0; i--) + { + for (int j = 0; j < maze->width; j++) + { + char cellType = maze->mazecells[j][i].type; + + switch (cellType) + { + case 'X': + printf("X "); // Wall + break; + case 'O': + printf("O "); // Obstacle + break; + case 'S': + printf("S "); // Start + break; + case 'G': + printf("G "); // Goal + break; + case 'C': + printf("C "); // Car + break; + case 'V': + printf("V "); // Visited + break; + default: + printf(" "); // Empty space + break; + } + } + printf("\n"); + } +} + + + +/** + * @brief Print the map with the reachable cells + * @param maze + */ +void +print_map_reachable(maze_t *maze) +{ + for (int i = maze->height - 1; i >= 0; i--) + { + for (int j = 0; j < maze->width; j++) + { + printf("%d ", maze->mazecells[j][i].reachable); + } + printf("\n"); + } +} + +/** + * @brief Perform floodfill on the maze + * @param maze + * @param x starting position x-coordinate + * @param y starting position y-coordinate + * @param value value to fill + */ +void +floodfill(maze_t *maze, int x, int y, int value) +{ + // Check if the current position is within the maze boundaries and not + // visited + if (x >= 0 && x < maze->width && y >= 0 && y < maze->height + && maze->mazecells[x][y].visited == 0) + { + maze->mazecells[x][y].reachable = value; + maze->mazecells[x][y].visited = 1; + + // Recursive floodfill for neighboring cells + floodfill(maze, x + 1, y, value + 1); // right + floodfill(maze, x - 1, y, value + 1); // left + floodfill(maze, x, y + 1, value + 1); // up + floodfill(maze, x, y - 1, value + 1); // down + } +} + +/** + * @brief Task to simulate the car moving in the maze and perform floodfill + * @param pvParameters + */ +void combined_task(void *pvParameters) +{ + maze_t *maze = (maze_t *)pvParameters; + int currentX = 0; // Initial X position + int currentY = 0; // Initial Y position + + for (;;) + { + // Reset maze before floodfill + for (int i = 0; i < maze->height; i++) + { + for (int j = 0; j < maze->width; j++) + { + maze->mazecells[j][i].visited = 0; + } + } + + // Simulate car movement (you can replace this logic with your actual + // movement algorithm) + mapping_direction_t moveDirection + = (mapping_direction_t)(get_rand_32() % 4); // Randomly choose a direction + + // Update the previously visited position before moving + maze->mazecells[currentX][currentY].type = 'V'; // 'V' for visited + + switch (moveDirection) + { + case up: + if (currentY < maze->height - 1 + && maze->mazecells[currentX][currentY + 1].type != 'X') + { + currentY++; + } + break; + case down: + if (currentY > 0 + && maze->mazecells[currentX][currentY - 1].type != 'X') + { + currentY--; + } + break; + case left: + if (currentX > 0 + && maze->mazecells[currentX - 1][currentY].type != 'X') + { + currentX--; + } + break; + case right: + if (currentX < maze->width - 1 + && maze->mazecells[currentX + 1][currentY].type != 'X') + { + currentX++; + } + break; + } + + // Update the car's position in the maze + // (you might want to clear the previous position before updating) + maze->mazecells[currentX][currentY].type = 'C'; // 'C' for car + + // Print the map with the car's position + printf("Map with the car's position:\n"); + print_map(maze); + + // Floodfill the maze after each movement + floodfill(maze, maze->width - 1, 0, 0); + + // Check if the car has reached the goal + if (maze->mazecells[currentX][currentY].type == 'G') + { + printf("Goal reached! Stopping the task.\n"); + // Stop the task + vTaskSuspend(NULL); + break; + } + + vTaskDelay(pdMS_TO_TICKS(100)); // Delay to simulate time between movements + } +} + +/** + * @brief Initialise tasks for the Maze + * @param maze + */ +void mapping_tasks_init(maze_t *maze) +{ + TaskHandle_t combined_task_handle = NULL; + xTaskCreate(combined_task, + "combined_task", + configMINIMAL_STACK_SIZE, + (void *)maze, + PRIO, + &combined_task_handle); +} + + +#endif /* MAPPING_H */