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

Merge remote-tracking branch 'origin/main'

This commit is contained in:
Richie 2023-11-09 13:20:45 +08:00
parent da028e41b1 1b8bcfefae
commit b7a0f60d8d
12 changed files with 221 additions and 448 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
frtos/car/car.h
View File

@ -7,14 +7,5 @@
#include "car_init.h"
static car_state_t initialize_car_state() {
g_car_state.x = MAP_SIZE >> 1;
g_car_state.y = MAP_SIZE >> 1;
g_car_state.current_direction = FORWARD;
g_car_state.orientation = NORTH;
return g_car_state;
}
#endif /* CAR_H */

28
frtos/car/car_init.h
View File

@ -12,33 +12,5 @@
#include "FreeRTOS.h"
#include "task.h"
#include "line_sensor_config.h"
typedef enum {
ERROR = 0,
RIGHT = 1,
LEFT = 2,
FORWARD = 3
} direction_t;
typedef enum {
NORTH = 0,
EAST = 1,
SOUTH = 2,
WEST = 3,
} orientation_t;
typedef struct {
u_int8_t x; // Current x coordinate
u_int8_t y; // Current y coordinate
direction_t current_direction; // Current direction (forward, left, right)
orientation_t orientation; // Current orientation (N, E, S, W)
} car_state_t;
/* Common Car State Structure (TODO: TBC)*/
static car_state_t g_car_state;
#endif /* CAR_INIT_H */

6
frtos/config/line_sensor_config.h
View File

@ -7,11 +7,5 @@
#define LEFT_SENSOR_PIN ( 26 )
#define RIGHT_SENSOR_PIN ( 27 )
#define BARCODE_SENSOR_PIN ( 22 )
/* Map */
#define MAP_START_SYMBOL ( 5 )
#define MAP_SIZE 20
#endif //CONFIG_H

13
frtos/config/ultrasonic_sensor_config.h
View File

@ -3,9 +3,14 @@
/* ADC Configuration */
#define TRIG_PIN ( 2 )
#define ECHO_PIN ( 3 )
#define TRIG_PIN (2)
#define ECHO_PIN (3)
#define ULTRASONIC_SENSOR_READ_DELAY ( 500 )
#define ULTRASONIC_SENSOR_READ_DELAY (100)
#endif //ULTRASONIC_CONFIG_H
typedef struct
{
bool obstacle_detected;
} ultrasonic_t;
#endif // ULTRASONIC_CONFIG_H

2
frtos/line_sensor/CMakeLists.txt
View File

@ -12,6 +12,8 @@ target_link_libraries(
target_include_directories(line_sensor_test
PRIVATE ../config
../car
../ultrasonic_sensor
)
pico_enable_stdio_usb(line_sensor_test 1)

175
frtos/line_sensor/line_sensor.h
View File

@ -4,58 +4,11 @@
* @author Woon Jun Wei
*/
#ifndef LINE_SENSOR_H
#define LINE_SENSOR_H
#include "line_sensor_init.h"
/**
* @brief Monitor the left sensor
*
* This function will monitor the left sensor and send the state to the
* left sensor message buffer, used to calculate the direction of the car
*
* @param params
*/
//void
//monitor_left_sensor_task(__unused void *params) {
// for (;;)
// {
// if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
// {
// if (left_sensor_triggered == pdTRUE)
// {
// printf("left sensor triggered\n");
// // Get Current State
// state_t state = gpio_get(LEFT_SENSOR_PIN);
//
// xMessageBufferSend(left_sensor_msg_buffer,
// &state,
// sizeof(state_t),
// 0);
// // Reset the flag
// left_sensor_triggered = pdFALSE;
// }
// }
// }
//}
void
monitor_left_sensor_task(__unused void *params) {
for (;;)
{
if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
{
printf("left sensor triggered\n");
// Get Current State
state_t state = gpio_get(LEFT_SENSOR_PIN);
xMessageBufferSend(left_sensor_msg_buffer,
&state,
sizeof(state_t),
0);
}
}
}
/**
* @brief Monitor the right sensor
*
@ -85,59 +38,25 @@ monitor_left_sensor_task(__unused void *params) {
// }
//}
void
monitor_right_sensor_task(void *params) {
for (;;) {
if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) {
// Check the flag or receive the message
printf("right sensor triggered\n");
// Get Current State
state_t state = gpio_get(RIGHT_SENSOR_PIN);
//void
//monitor_right_sensor_task(void *pvParameters) {
//
// volatile
// for (;;) {
// if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) {
// // Check the flag or receive the message
// printf("right sensor triggered\n");
// // Get Current State
// state_t state = gpio_get(RIGHT_SENSOR_PIN);
//
//// xMessageBufferSend(right_sensor_msg_buffer,
//// &state,
//// sizeof(state_t),
//// 0);
// }
// }
//}
xMessageBufferSend(right_sensor_msg_buffer,
&state,
sizeof(state_t),
0);
}
}
}
/**
* @brief Monitor the barcode sensor
*
* This function will monitor the barcode sensor and send the state to the
* barcode sensor message buffer, used to scan the barcode below the car
*
* @param params
*/
void monitor_barcode_sensor_task(void *params) {
for (;;) {
if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) {
// Check the flag or receive the message
if (barcode_sensor_triggered == pdTRUE) {
uint32_t barcode_data = 0;
for (int i = 0; i < 9; i++) {
sleep_ms(100); // Wait for a segment of the barcode
if (gpio_get(BARCODE_SENSOR_PIN)) {
barcode_data |= (1u << i);
} else {
barcode_data &= ~(1u << i);
}
}
printf("Barcode Data (binary): %09b\n", barcode_data);
// Send or process the barcode data
xMessageBufferSend(barcode_sensor_msg_buffer, &barcode_data, sizeof(uint32_t), 0);
// Reset the flag
barcode_sensor_triggered = pdFALSE;
}
}
}
}
/**
* @brief Monitor the direction and Oritentation of the car
*
@ -146,29 +65,29 @@ void monitor_barcode_sensor_task(void *params) {
*
* @param params
*/
void
monitor_direction_task(__unused void *params) {
state_t left_state;
state_t right_state;
state_t barcode_state;
for (;;)
{
// Receive from Buffer
xMessageBufferReceive(left_sensor_msg_buffer,
&left_state,
sizeof(state_t),
portMAX_DELAY);
xMessageBufferReceive(right_sensor_msg_buffer,
&right_state,
sizeof(state_t),
portMAX_DELAY);
xMessageBufferReceive(barcode_sensor_msg_buffer,
&barcode_state,
sizeof(state_t),
portMAX_DELAY);
//void
//monitor_direction_task(__unused void *params) {
// state_t left_state;
// state_t right_state;
// state_t barcode_state;
//
// for (;;)
// {
// // Receive from Buffer
// xMessageBufferReceive(left_sensor_msg_buffer,
// &left_state,
// sizeof(state_t),
// portMAX_DELAY);
//
// xMessageBufferReceive(right_sensor_msg_buffer,
// &right_state,
// sizeof(state_t),
// portMAX_DELAY);
//
// xMessageBufferReceive(barcode_sensor_msg_buffer,
// &barcode_state,
// sizeof(state_t),
// portMAX_DELAY);
// g_car_state.current_direction = (left_state << 1) | right_state;
@ -220,5 +139,7 @@ monitor_direction_task(__unused void *params) {
// printf("Orientation: Error\n");
// break;
// }
}
}
// }
//}
#endif /* LINE_SENSOR_H */

173
frtos/line_sensor/line_sensor_init.h
View File

@ -18,64 +18,10 @@
#include "semphr.h"
#include "line_sensor_config.h"
#define DEBOUNCE_DELAY_MS 100
static TickType_t lastEdgeTimeLeft = 0;
static TickType_t lastEdgeTimeRight = 0;
static TickType_t lastBarcodeTime = 0;
typedef enum { // Unused, useful for readability
LINE_DETECTED = 0,
LINE_NOT_DETECTED = 1,
} state_t;
//typedef enum {
// ERROR = 0,
// RIGHT = 1,
// LEFT = 2,
// FORWARD = 3
//} direction_t;
//typedef enum {
// NORTH = 0,
// EAST = 1,
// SOUTH = 2,
// WEST = 3,
//} orientation_t;
//typedef struct {
// uint8_t x; // Current x coordinate
// uint8_t y; // Current y coordinate
// direction_t current_direction; // Current direction (forward, left, right)
// orientation_t orientation; // Current orientation (N, E, S, W)
//} car_state_t;
#include "car_config.h"
// Semaphore
SemaphoreHandle_t g_left_sensor_sem = NULL;
SemaphoreHandle_t g_right_sensor_sem = NULL;
SemaphoreHandle_t g_barcode_sensor_sem = NULL;
// Queue
static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer
static MessageBufferHandle_t right_sensor_msg_buffer; // Right Sensor Buffer
static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer
static volatile BaseType_t right_sensor_triggered = pdFALSE;
static volatile BaseType_t left_sensor_triggered = pdFALSE;
static volatile BaseType_t barcode_sensor_triggered = pdFALSE;
//// Car State Struct
//static car_state_t g_car_state;
//
//static car_state_t initialize_car_state() {
// g_car_state.x = MAP_SIZE >> 1;
// g_car_state.y = MAP_SIZE >> 1;
// g_car_state.current_direction = FORWARD;
// g_car_state.orientation = NORTH;
//
// return g_car_state;
//}
/**
* @brief Setup the Line Sensor
@ -83,21 +29,19 @@ static volatile BaseType_t barcode_sensor_triggered = pdFALSE;
* This function will setup the Line Sensor by initializing it as an input
*/
static inline void
line_sensor_setup() {
g_left_sensor_sem = xSemaphoreCreateBinary();
g_right_sensor_sem = xSemaphoreCreateBinary();
g_barcode_sensor_sem = xSemaphoreCreateBinary();
line_sensor_init() {
// obs_t obs = {0, 0};
//
// p_car->obs = &obs;
uint mask = (1 << LEFT_SENSOR_PIN) | (1 << RIGHT_SENSOR_PIN) | (1 << BARCODE_SENSOR_PIN);
g_left_sensor_sem = xSemaphoreCreateBinary();
uint mask = (1 << LEFT_SENSOR_PIN) | (1 << RIGHT_SENSOR_PIN);
// Initialise 3 GPIO pins and set them to input
gpio_init_mask(mask);
gpio_set_dir_in_masked(mask);
left_sensor_msg_buffer = xMessageBufferCreate(30);
right_sensor_msg_buffer = xMessageBufferCreate(30);
barcode_sensor_msg_buffer = xMessageBufferCreate(30);
}
/**
@ -114,100 +58,23 @@ bool h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
return true;
}
/**
* @brief Timer Interrupt Handler for the right sensor
*
* @param repeatingTimer
* @return True (To keep the timer running)
*/
bool h_right_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_right_sensor_sem,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
void
monitor_left_sensor_task(void *pvParameters) {
volatile obs_t *p_obs = NULL;
p_obs = (obs_t *) pvParameters;
return true;
}
/**
* @brief Timer Interrupt Handler for the barcode sensor
*
* @param repeatingTimer
* @return True (To keep the timer running)
*/
bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_barcode_sensor_sem,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true;
}
void h_line_sensor_handler(void) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
TickType_t currentTicks = xTaskGetTickCount();
printf("Interrupt triggered\n");
if (gpio_get_irq_event_mask(LEFT_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL)
for (;;)
{
if ((currentTicks - lastEdgeTimeLeft) >=
pdMS_TO_TICKS(DEBOUNCE_DELAY_MS))
{
lastEdgeTimeLeft = currentTicks;
gpio_acknowledge_irq(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL);
left_sensor_triggered = pdTRUE;
xSemaphoreGiveFromISR(g_left_sensor_sem, &xHigherPriorityTaskWoken);
}
else
{
// Reset the timer to the currentTicks if the edge is ignored
lastEdgeTimeLeft = currentTicks;
}
}
if (gpio_get_irq_event_mask(RIGHT_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL)
{
if ((currentTicks - lastEdgeTimeRight) >=
pdMS_TO_TICKS(DEBOUNCE_DELAY_MS))
{
lastEdgeTimeRight = currentTicks;
gpio_acknowledge_irq(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL);
// if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
// {
// Set the flag to notify the task
right_sensor_triggered = pdTRUE;
xSemaphoreGiveFromISR(g_right_sensor_sem,
&xHigherPriorityTaskWoken);
p_obs->line_detected = gpio_get(LEFT_SENSOR_PIN);
printf("Left Sensor: %d\n", p_obs->line_detected);
vTaskDelay(pdMS_TO_TICKS(LINE_SENSOR_READ_DELAY));
// }
}
else
{
// Reset the timer to the currentTicks if the edge is ignored
lastEdgeTimeRight = currentTicks;
}
}
if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL)
{
if ((currentTicks - lastBarcodeTime) >=
pdMS_TO_TICKS(DEBOUNCE_DELAY_MS))
{
lastBarcodeTime = currentTicks;
gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL);
// Set the flag to notify the task
barcode_sensor_triggered = pdTRUE;
xSemaphoreGiveFromISR(g_barcode_sensor_sem,
&xHigherPriorityTaskWoken);
}
else
{
// Reset the timer to the currentTicks if the edge is ignored
lastBarcodeTime = currentTicks;
}
}
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
#endif /* LINE_SENSOR_INIT_H */

92
frtos/line_sensor/line_sensor_test.c
View File

@ -1,88 +1,52 @@
#include "line_sensor.h"
#include "line_sensor_init.h"
#include "ultrasonic_sensor.h"
#include "car_config.h"
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
void
launch()
launch(car_struct_t *car_struct)
{
// isr to detect left line sensor
gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler);
// isr to detect right line sensor
gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler);
// isr to detect barcode line sensor
gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_line_sensor_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
struct repeating_timer g_left_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_left_sensor_timer_handler,
NULL,
&g_left_sensor_timer);
struct repeating_timer g_right_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_right_sensor_timer_handler,
NULL,
&g_right_sensor_timer);
TaskHandle_t h_monitor_left_sensor_task;
TaskHandle_t h_monitor_left_sensor_task = NULL;
xTaskCreate(monitor_left_sensor_task,
"Monitor Left Sensor Task",
"read_left_sensor_task",
configMINIMAL_STACK_SIZE,
NULL,
(void *)car_struct->obs,
READ_LEFT_SENSOR_PRIO,
&h_monitor_left_sensor_task);
TaskHandle_t h_monitor_right_sensor_task;
xTaskCreate(monitor_right_sensor_task,
"Monitor Right Sensor Task",
TaskHandle_t h_monitor_ultrasonic_task = NULL;
xTaskCreate(check_obstacle,
"read_ultrasonic_task",
configMINIMAL_STACK_SIZE,
NULL,
READ_RIGHT_SENSOR_PRIO,
&h_monitor_right_sensor_task);
TaskHandle_t h_monitor_barcode_sensor_task;
xTaskCreate(monitor_barcode_sensor_task,
"Monitor Barcode Sensor Task",
configMINIMAL_STACK_SIZE,
NULL,
READ_RIGHT_SENSOR_PRIO,
&h_monitor_right_sensor_task);
// TaskHandle_t h_monitor_direction_task;
// xTaskCreate(monitor_direction_task,
// "Monitor Direction Task",
// configMINIMAL_STACK_SIZE,
// NULL,
// DIRECTION_TASK_PRIORITY,
// &h_monitor_direction_task);
vTaskStartScheduler();
(void *)car_struct->obs,
READ_LEFT_SENSOR_PRIO,
&h_monitor_ultrasonic_task);
}
int
main (void)
main(void)
{
stdio_usb_init();
// sleep_ms(2000);
obs_t obs = { 0, 0 };
car_struct_t car_struct = { .obs = &obs };
sleep_ms(2000);
printf("Test started!\n");
line_sensor_setup();
initialize_car_state();
line_sensor_init();
printf("Line sensor initialized!\n");
launch();
init_ultrasonic();
printf("Ultrasonic sensor initialized!\n");
launch(&car_struct);
vTaskStartScheduler();
return (0);
}

2
frtos/magnetometer/magnetometer_direction.h
View File

@ -340,7 +340,7 @@ void updateDirection() {
// Temperature in degrees Celsius
// printf("Temperature: %d\n", temperature[0]);
print_orientation_data();
// print_orientation_data();
// printf("Direction: ");

2
frtos/ultrasonic_sensor/ultrasonic_init.h
View File

@ -11,6 +11,8 @@
#include "pico/stdlib.h"
#include "ultrasonic_sensor_config.h"
ultrasonic_t ultrasonic_sensor = { .obstacle_detected = false };
void
init_ultrasonic(void)
{

6
frtos/ultrasonic_sensor/ultrasonic_sensor.c
View File

@ -8,12 +8,12 @@
void
vLaunch(void)
{
gpio_set_irq_enabled_with_callback(ECHO_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, echo_handler);
// gpio_set_irq_enabled_with_callback(ECHO_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, echo_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
// irq_set_enabled(IO_IRQ_BANK0, true);
TaskHandle_t disttask;
xTaskCreate(distance_task,
xTaskCreate(check_obstacle,
"TestDistThread",
configMINIMAL_STACK_SIZE,
NULL,

101
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -1,18 +1,19 @@
/**
* @file ultrasonic_sensor.h
* @brief Monitor the distance between the car and the wall
* @author Poon Xiang Yuan
*/
* @file ultrasonic_sensor.h
* @brief Monitor the distance between the car and the wall
* @author Poon Xiang Yuan
*/
#ifndef ULTRASONIC_SENSOR_H
#define ULTRASONIC_SENSOR_H
#include "ultrasonic_init.h"
#include "motor_speed.h"
//#include "motor_speed.h"
#include "car_config.h"
// volatile uint32_t start_time;
// volatile uint32_t end_time;
volatile bool echo_rising = false;
// volatile bool echo_rising = false;
float
KalmanFilter(float U)
@ -52,16 +53,15 @@ KalmanFilter(float U)
// }
void
distance_task(__unused void *params)
check_obstacle(void *pvParameters)
{
while (true)
{
vTaskDelay(1000);
{ // Put trigger pin high for 10us
gpio_put(TRIG_PIN, 1);
sleep_us(10);
gpio_put(TRIG_PIN, 0);
// Wait for echo pin to go high
while (gpio_get(ECHO_PIN) == 0)
tight_loop_contents();
@ -69,6 +69,7 @@ distance_task(__unused void *params)
uint32_t start_time = time_us_32();
while (gpio_get(ECHO_PIN) == 1)
tight_loop_contents();
uint32_t end_time = time_us_32();
// Calculate the distance (in centimeters)
@ -76,20 +77,74 @@ distance_task(__unused void *params)
float distance
= (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
printf("Distance: %.2f cm\n", distance);
// printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
// printf("Distance: %.2f cm\n", distance);
if (distance < 7)
{
// set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u);
printf("Collision Imminent!\n");
vTaskDelay(pdMS_TO_TICKS(3000));
spin_to_yaw(350);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u);
}
start_time, end_time = 0;
// change value of obstacle_detected in ultrasonic_t struct
obs_t *ultrasonic_sensor = (obs_t *)pvParameters;
ultrasonic_sensor->ultrasonic_detected = (distance < 7);
printf("Distance: %.2f cm, Obstacle Detected: %d\n",
distance,
ultrasonic_sensor->ultrasonic_detected);
vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
}
}
//void
//check_global(void *pvParameters)
//{
// while (true)
// {
// ultrasonic_t *ultrasonic_sensor = (ultrasonic_t *)pvParameters;
//
// printf("Global Obstacle Detected : %d\n",
// ultrasonic_sensor->obstacle_detected);
// vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
// }
//}
// void
// distance_task(__unused void *params)
// {
// while (true)
// {
// vTaskDelay(1000);
// gpio_put(TRIG_PIN, 1);
// sleep_us(10);
// gpio_put(TRIG_PIN, 0);
// while (gpio_get(ECHO_PIN) == 0)
// tight_loop_contents();
// // Measure the pulse width (time taken for the echo to return)
// uint32_t start_time = time_us_32();
// while (gpio_get(ECHO_PIN) == 1)
// tight_loop_contents();
// uint32_t end_time = time_us_32();
// // Calculate the distance (in centimeters)
// uint32_t pulse_duration = end_time - start_time;
// float distance
// = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
// printf("Distance: %.2f cm\n", distance);
// // printf("Kalman Filtered Distance: %.2f cm\n",
// // KalmanFilter(distance));
// if (distance < 7)
// {
// // set_wheel_direction(DIRECTION_MASK);
// set_wheel_speed_synced(0u);
// printf("Collision Imminent!\n");
// vTaskDelay(pdMS_TO_TICKS(3000));
// spin_to_yaw(350);
// set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed_synced(90u);
// }
// start_time, end_time = 0;
// }
// }
#endif /* ULTRASONIC_SENSOR_H */