736 lines
20 KiB
C
736 lines
20 KiB
C
/**
|
|
* @file mapping.h
|
|
* @brief Map the environment using the line sensor and the ultrasonic sensor
|
|
*
|
|
* Reference:
|
|
* https://stackoverflow.com/questions/37207022/flood-fill-algorithm-maze
|
|
*
|
|
* @author Woon Jun Wei
|
|
*/
|
|
|
|
#ifndef MAPPING_H
|
|
#define MAPPING_H
|
|
|
|
#include <stdio.h>
|
|
#include "pico/stdlib.h"
|
|
#include <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;
|
|
}
|
|
|
|
// Define a queue structure for BFS
|
|
typedef struct
|
|
{
|
|
int x;
|
|
int y;
|
|
} QueueNode;
|
|
|
|
typedef struct
|
|
{
|
|
QueueNode *array;
|
|
int front, rear, size;
|
|
unsigned capacity;
|
|
} Queue;
|
|
|
|
// Function to create a new queue
|
|
Queue *
|
|
createQueue(unsigned capacity)
|
|
{
|
|
Queue *queue = (Queue *)malloc(sizeof(Queue));
|
|
queue->capacity = capacity;
|
|
queue->front = queue->size = 0;
|
|
queue->rear = capacity - 1;
|
|
queue->array = (QueueNode *)malloc(capacity * sizeof(QueueNode));
|
|
return queue;
|
|
}
|
|
|
|
// Function to check if the queue is empty
|
|
bool
|
|
isEmpty(Queue *queue)
|
|
{
|
|
return (queue->size == 0);
|
|
}
|
|
|
|
// Function to check if the queue is full
|
|
bool
|
|
isFull(Queue *queue)
|
|
{
|
|
return (queue->size == queue->capacity);
|
|
}
|
|
|
|
// Function to enqueue a cell in the queue
|
|
void
|
|
enqueue(Queue *queue, int x, int y)
|
|
{
|
|
if (isFull(queue))
|
|
return;
|
|
queue->rear = (queue->rear + 1) % queue->capacity;
|
|
queue->array[queue->rear].x = x;
|
|
queue->array[queue->rear].y = y;
|
|
queue->size = queue->size + 1;
|
|
}
|
|
|
|
// Function to dequeue a cell from the queue
|
|
QueueNode
|
|
dequeue(Queue *queue)
|
|
{
|
|
QueueNode cell = queue->array[queue->front];
|
|
queue->front = (queue->front + 1) % queue->capacity;
|
|
queue->size = queue->size - 1;
|
|
return cell;
|
|
}
|
|
|
|
// Function to perform BFS and find the shortest path
|
|
void
|
|
bfs_shortest_path(maze_t *maze, int startX, int startY)
|
|
{
|
|
// Create a queue for BFS
|
|
Queue *queue = createQueue(maze->height * maze->width);
|
|
|
|
// Initialize visited array
|
|
bool visited[maze->height][maze->width];
|
|
for (int i = 0; i < maze->height; i++)
|
|
{
|
|
for (int j = 0; j < maze->width; j++)
|
|
{
|
|
visited[i][j] = false;
|
|
}
|
|
}
|
|
|
|
// Mark the starting cell as visited and enqueue it
|
|
visited[startY][startX] = true;
|
|
enqueue(queue, startX, startY);
|
|
|
|
// Define directions (up, down, left, right)
|
|
int dx[] = { -1, 1, 0, 0 };
|
|
int dy[] = { 0, 0, -1, 1 };
|
|
|
|
// Perform BFS
|
|
while (!isEmpty(queue))
|
|
{
|
|
// Dequeue a cell and process it
|
|
QueueNode current = dequeue(queue);
|
|
int x = current.x;
|
|
int y = current.y;
|
|
|
|
// Process the cell (you can customize this part based on your needs)
|
|
// Here, we mark the cell with a special character to indicate it's part
|
|
// of the shortest path
|
|
maze->mazecells[y][x].type = 'P'; // 'P' for path
|
|
|
|
// Explore adjacent cells
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
int newX = x + dx[i];
|
|
int newY = y + dy[i];
|
|
|
|
// Check if the new position is within the maze boundaries
|
|
if (newX >= 0 && newX < maze->width && newY >= 0
|
|
&& newY < maze->height)
|
|
{
|
|
// Check if the cell is not a wall and hasn't been visited
|
|
if (maze->mazecells[newY][newX].type != 'X'
|
|
&& !visited[newY][newX])
|
|
{
|
|
// Mark the new cell as visited and enqueue it
|
|
visited[newY][newX] = true;
|
|
enqueue(queue, newX, newY);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Free the allocated memory for the queue
|
|
free(queue);
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @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");
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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 based on the image
|
|
char hardcoded_map[5][5] = { { ' ', ' ', ' ', ' ', ' ' },
|
|
{ 'S', 'X', 'X', 'X', ' ' },
|
|
{ ' ', 'X', ' ', ' ', ' ' },
|
|
{ ' ', 'X', ' ', 'X', ' ' },
|
|
{ ' ', ' ', ' ', 'X', 'G' } };
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
printf("Here is the hardcoded map:\n");
|
|
print_map(maze);
|
|
}
|
|
|
|
/**
|
|
* @brief Mapping Initialization
|
|
*/
|
|
void
|
|
mapping_init(maze_t *p_maze)
|
|
{
|
|
printf("Initializing mapping\n");
|
|
|
|
// Set fixed height and width during initialization
|
|
p_maze->height = 5;
|
|
p_maze->width = 5;
|
|
|
|
// Create the maze
|
|
printf("Creating maze\n");
|
|
create_hardcoded_map(p_maze);
|
|
printf("Maze created\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 Function to check if the entire map is filled
|
|
* @param maze
|
|
* @return true if the entire map is filled, false otherwise
|
|
*/
|
|
bool
|
|
maze_explored(const maze_t *maze)
|
|
{
|
|
for (int i = 0; i < maze->height; i++)
|
|
{
|
|
for (int j = 0; j < maze->width; j++)
|
|
{
|
|
if (maze->mazecells[j][i].type != 'X'
|
|
&& maze->mazecells[j][i].type != 'V'
|
|
&& maze->mazecells[j][i].type != 'C'
|
|
&& maze->mazecells[j][i].type != 'G'
|
|
&& maze->mazecells[j][i].type != 'S')
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// Update the find_shortest_path function with the newly created
|
|
// bfs_shortest_path function
|
|
void
|
|
find_shortest_path(maze_t *maze)
|
|
{
|
|
// Assuming the starting point is the bottom-left corner (0, 0)
|
|
int startX = 0;
|
|
int startY = 0;
|
|
|
|
// Perform BFS to find the shortest path
|
|
bfs_shortest_path(maze, startX, startY);
|
|
}
|
|
|
|
/**
|
|
* @brief Function to backtrack to the start from the goal iteratively
|
|
* @param maze
|
|
* @param currentX pointer to the current X position
|
|
* @param currentY pointer to the current Y position
|
|
*/
|
|
void
|
|
backtrack_to_start(maze_t *maze, int *currentX, int *currentY)
|
|
{
|
|
printf("Backtracking to the start...\n");
|
|
|
|
// Continue backtracking until reaching the start
|
|
while (*currentX != 0 || *currentY != 0)
|
|
{
|
|
printf("Backtracking...\n");
|
|
print_map(maze);
|
|
|
|
// Update the current cell as part of the backtracking path
|
|
maze->mazecells[*currentX][*currentY].type = 'P'; // 'P' for path
|
|
|
|
// Move the car towards the starting point
|
|
if (*currentX > 0)
|
|
{
|
|
(*currentX)--;
|
|
}
|
|
else if (*currentY > 0)
|
|
{
|
|
(*currentY)--;
|
|
}
|
|
|
|
// Print the map after updating the current cell during backtracking
|
|
printf("Map after updating current cell during backtracking:\n");
|
|
print_map(maze);
|
|
|
|
// Print the car's position in the map
|
|
printf("Car's position during backtracking: (%d, %d)\n",
|
|
*currentX,
|
|
*currentY);
|
|
|
|
vTaskDelay(
|
|
pdMS_TO_TICKS(100)); // Delay to simulate time between movements
|
|
}
|
|
|
|
printf("Backtracking completed. Reached the start!\n");
|
|
}
|
|
|
|
/**
|
|
* @brief Task to demonstrate the car following the shortest path from start to
|
|
* goal
|
|
* @param pvParameters
|
|
*/
|
|
void
|
|
demo_shortest_path_task(void *pvParameters)
|
|
{
|
|
maze_t *maze = (maze_t *)pvParameters;
|
|
|
|
// Assuming the starting point is the bottom-left corner (0, 0)
|
|
int currentX = 0;
|
|
int currentY = 0;
|
|
|
|
// Find the shortest path using BFS
|
|
bfs_shortest_path(maze, currentX, currentY);
|
|
|
|
printf("Shortest path found. Demonstrating the car's movement...\n");
|
|
|
|
// Iterate through the path and demonstrate the car's movement
|
|
for (int i = maze->height - 1; i >= 0; i--)
|
|
{
|
|
for (int j = 0; j < maze->width; j++)
|
|
{
|
|
if (maze->mazecells[i][j].type == 'P')
|
|
{
|
|
// Move the car to the cell in the shortest path
|
|
currentX = j;
|
|
currentY = i;
|
|
|
|
// Print the map with the car's position
|
|
printf("Map with the car's position (BFS):\n");
|
|
print_map(maze);
|
|
|
|
// Delay to simulate the car's movement
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
}
|
|
}
|
|
}
|
|
|
|
printf("Car reached the goal following the shortest path!\n");
|
|
|
|
vTaskDelete(NULL); // Delete the demonstration task
|
|
}
|
|
|
|
/**
|
|
* @brief Task to perform mapping of the maze
|
|
* @param pvParameters
|
|
*/
|
|
void
|
|
mapping_task(void *pvParameters)
|
|
{
|
|
maze_t *maze = (maze_t *)pvParameters;
|
|
int currentX = 0; // Initial X position
|
|
int currentY = 0; // Initial Y position
|
|
|
|
// Reset maze before mapping
|
|
for (int i = 0; i < maze->height; i++)
|
|
{
|
|
for (int j = 0; j < maze->width; j++)
|
|
{
|
|
maze->mazecells[j][i].visited = 0;
|
|
}
|
|
}
|
|
|
|
// Explore the maze and perform floodfill
|
|
for (;;)
|
|
{
|
|
// Simulate car movement (you can replace this logic with your actual
|
|
// movement algorithm)
|
|
|
|
mapping_direction_t moveDirection
|
|
= (mapping_direction_t)(get_rand_32() % 4);
|
|
|
|
// Update the previously visited position before moving
|
|
if (maze->mazecells[currentX][currentY].type != 'S'
|
|
&& maze->mazecells[currentX][currentY].type != 'G')
|
|
{
|
|
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
|
|
if (maze->mazecells[currentX][currentY].type != 'S')
|
|
{
|
|
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 explored the entire maze
|
|
printf("%d\n", maze_explored(maze));
|
|
if (maze_explored(maze))
|
|
{
|
|
printf("Entire maze explored!\n");
|
|
|
|
// Continue with backtracking, BFS, and demonstration of the
|
|
// shortest path
|
|
printf("Now Backtracking...\n");
|
|
backtrack_to_start(maze, ¤tX, ¤tY);
|
|
|
|
printf("Map after backtracking:\n");
|
|
print_map(maze);
|
|
|
|
// Find the shortest path after backtracking
|
|
printf("Finding the shortest path...\n");
|
|
find_shortest_path(maze);
|
|
|
|
// Create a task to demonstrate the shortest path
|
|
xTaskCreate(demo_shortest_path_task,
|
|
"demo_shortest_path_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)maze,
|
|
PRIO,
|
|
NULL);
|
|
}
|
|
|
|
vTaskDelay(
|
|
pdMS_TO_TICKS(100)); // Delay to simulate time between movements
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Task to perform backtracking from the goal to the start
|
|
* @param pvParameters
|
|
*/
|
|
void
|
|
backtracking_task(void *pvParameters)
|
|
{
|
|
maze_t *maze = (maze_t *)pvParameters;
|
|
|
|
int currentX = 0; // Initial X position
|
|
int currentY = 0; // Initial Y position
|
|
|
|
printf("Backtracking to the start...\n");
|
|
backtrack_to_start(maze, ¤tX, ¤tY);
|
|
|
|
printf("Map after backtracking:\n");
|
|
print_map(maze);
|
|
|
|
vTaskDelete(NULL); // Delete the backtracking task
|
|
}
|
|
|
|
/**
|
|
* @brief Task to show the movement from start to goal
|
|
* @param pvParameters
|
|
*/
|
|
void
|
|
movement_task(void *pvParameters)
|
|
{
|
|
maze_t *maze = (maze_t *)pvParameters;
|
|
|
|
int currentX = 0; // Initial X position
|
|
int currentY = 0; // Initial Y position
|
|
|
|
for (;;)
|
|
{
|
|
// Simulate car movement (you can replace this logic with your actual
|
|
// movement algorithm)
|
|
mapping_direction_t moveDirection
|
|
= (mapping_direction_t)(get_rand_32() % 4);
|
|
|
|
// Update the previously visited position before moving
|
|
if (maze->mazecells[currentX][currentY].type != 'S'
|
|
&& maze->mazecells[currentX][currentY].type != 'G')
|
|
{
|
|
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
|
|
if (maze->mazecells[currentX][currentY].type != 'S')
|
|
{
|
|
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);
|
|
|
|
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)
|
|
{
|
|
// Task handles
|
|
TaskHandle_t mapping_task_handle = NULL;
|
|
TaskHandle_t backtracking_task_handle = NULL;
|
|
TaskHandle_t demo_shortest_path_handle = NULL;
|
|
TaskHandle_t movement_task_handle = NULL;
|
|
|
|
// Create tasks
|
|
xTaskCreate(mapping_task,
|
|
"mapping_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)maze,
|
|
PRIO,
|
|
&mapping_task_handle);
|
|
|
|
xTaskCreate(backtracking_task,
|
|
"backtracking_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)maze,
|
|
PRIO,
|
|
&backtracking_task_handle);
|
|
|
|
// Shortest path task demo
|
|
xTaskCreate(demo_shortest_path_task,
|
|
"demo_shortest_path_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)maze,
|
|
PRIO,
|
|
&demo_shortest_path_handle);
|
|
|
|
xTaskCreate(movement_task,
|
|
"movement_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)maze,
|
|
PRIO,
|
|
&movement_task_handle);
|
|
|
|
// Suspend all tasks except the mapping task
|
|
vTaskSuspend(backtracking_task_handle);
|
|
vTaskSuspend(demo_shortest_path_handle);
|
|
vTaskSuspend(movement_task_handle);
|
|
}
|
|
|
|
#endif /* MAPPING_H */
|