diff --git a/frtos/car/car.h b/frtos/car/car.h index b0340cf..6b280ab 100644 --- a/frtos/car/car.h +++ b/frtos/car/car.h @@ -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 */ \ No newline at end of file diff --git a/frtos/car/car_init.h b/frtos/car/car_init.h index 11cab94..c576ea2 100644 --- a/frtos/car/car_init.h +++ b/frtos/car/car_init.h @@ -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 */ \ No newline at end of file diff --git a/frtos/config/car_config.h b/frtos/config/car_config.h new file mode 100644 index 0000000..6e1a9de --- /dev/null +++ b/frtos/config/car_config.h @@ -0,0 +1,27 @@ +#include "motor_config.h" +#include "ultrasonic_sensor_config.h" +#include "magnetometer_config.h" +#include "line_sensor_config.h" + +#ifndef CAR_CONFIG_H +#define CAR_CONFIG_H + +#define PRIO (tskIDLE_PRIORITY + 1UL) + +typedef struct s_obs_struct +{ + bool line_detected; + bool ultrasonic_detected; + +} obs_t; + +typedef struct +{ + obs_t *obs; + motor_t *p_left_motor; + motor_t *p_right_motor; + motor_pid_t *p_pid; + +} car_struct_t; + +#endif // CAR_CONFIG_H diff --git a/frtos/config/line_sensor_config.h b/frtos/config/line_sensor_config.h index 68ec354..007cc77 100644 --- a/frtos/config/line_sensor_config.h +++ b/frtos/config/line_sensor_config.h @@ -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 diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 6aafd4f..43ad6d1 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -34,10 +34,6 @@ #define MAX_PWM_LEVEL 99U #define MIN_PWM_LEVEL 0U -#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) -#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) -#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 1UL) - /*! * @brief Structure for the motor speed parameters * @param current_speed_cms Current speed in cm/s @@ -47,6 +43,7 @@ typedef struct { float current_cms; float distance_cm; + } motor_speed_t; /*! @@ -67,6 +64,7 @@ typedef struct * @param pid_kp Proportional gain * @param pid_ki Integral gain * @param pid_kd Derivative gain + * @param use_pid Flag to use PID or not */ typedef struct { @@ -79,9 +77,9 @@ typedef struct /*! * @brief Structure for the motor parameters * @param speed Motor speed parameters - * @param sem Semaphore for the motor speed * @param pwm Motor PWM parameters - * @param pid Motor PID parameters + * @param p_sem Pointer to the semaphore + * @param use_pid Pointer to the use_pid flag */ typedef struct { @@ -102,12 +100,13 @@ typedef struct SemaphoreHandle_t g_left_sem; SemaphoreHandle_t g_right_sem; -typedef struct -{ - motor_t * p_left_motor; - motor_t * p_right_motor; - motor_pid_t * p_pid; - -} car_struct_t; +// for testing +//typedef struct +//{ +// motor_t * p_left_motor; +// motor_t * p_right_motor; +// motor_pid_t * p_pid; +// +//} car_struct_t; #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/config/ultrasonic_sensor_config.h b/frtos/config/ultrasonic_sensor_config.h index 3bdcc22..2f38225 100644 --- a/frtos/config/ultrasonic_sensor_config.h +++ b/frtos/config/ultrasonic_sensor_config.h @@ -3,9 +3,9 @@ /* 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 +#endif // ULTRASONIC_CONFIG_H diff --git a/frtos/line_sensor/CMakeLists.txt b/frtos/line_sensor/CMakeLists.txt index 05b63ca..3ca578d 100644 --- a/frtos/line_sensor/CMakeLists.txt +++ b/frtos/line_sensor/CMakeLists.txt @@ -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) diff --git a/frtos/line_sensor/line_sensor.h b/frtos/line_sensor/line_sensor.h deleted file mode 100644 index 9aa8970..0000000 --- a/frtos/line_sensor/line_sensor.h +++ /dev/null @@ -1,224 +0,0 @@ -/** - * @file line_sensor.h - * @brief Monitor the line sensor and update the car state accordingly - * @author Woon Jun Wei - */ - -#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 - * - * This function will monitor the right sensor and send the state to the - * right sensor message buffer, used to calculate the direction of the car - * - * @param 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 -// if (right_sensor_triggered == pdTRUE) { -// 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); -// // Reset the flag -// right_sensor_triggered = pdFALSE; -// } -// } -// } -//} - -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); - - 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 - * - * This function will monitor the direction and orientation of the car - * and update the car state accordingly - * - * @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); - -// g_car_state.current_direction = (left_state << 1) | right_state; - -// switch (g_car_state.current_direction) -// { -// case FORWARD: -// break; -// case RIGHT: -// g_car_state.orientation = (g_car_state.orientation + 1) & 0x03; -// break; -// case LEFT: -// g_car_state.orientation = (g_car_state.orientation - 1) & 0x03; -// break; -// default: -// break; -// } -// -// switch (g_car_state.current_direction) -// { -// case FORWARD: -// printf("Direction: Forward\n"); -// break; -// case RIGHT: -// printf("Direction: Right\n"); -// break; -// case LEFT: -// printf("Direction: Left\n"); -// break; -// default: -// printf("Direction: Error\n"); -// break; -// } -// -// switch (g_car_state.orientation) -// { -// case NORTH: -// printf("Orientation: North\n"); -// break; -// case EAST: -// printf("Orientation: East\n"); -// break; -// case SOUTH: -// printf("Orientation: South\n"); -// break; -// case WEST: -// printf("Orientation: West\n"); -// break; -// default: -// printf("Orientation: Error\n"); -// break; -// } - } -} \ No newline at end of file diff --git a/frtos/line_sensor/line_sensor_init.h b/frtos/line_sensor/line_sensor_init.h index b15447a..69b0b13 100644 --- a/frtos/line_sensor/line_sensor_init.h +++ b/frtos/line_sensor/line_sensor_init.h @@ -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,18 @@ 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(car_struct_t *p_car_struct) +{ + p_car_struct->obs->line_detected = false; - 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 +57,35 @@ 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); - } - else - { - // Reset the timer to the currentTicks if the edge is ignored - lastEdgeTimeRight = currentTicks; - } + 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)); +// } } - - 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); } +void +line_tasks_init(car_struct_t *car_struct) +{ + TaskHandle_t h_monitor_left_sensor_task = NULL; + xTaskCreate(monitor_left_sensor_task, + "read_left_sensor_task", + configMINIMAL_STACK_SIZE, + (void *)car_struct->obs, + PRIO, + &h_monitor_left_sensor_task); +} + + #endif /* LINE_SENSOR_INIT_H */ diff --git a/frtos/line_sensor/line_sensor_test.c b/frtos/line_sensor/line_sensor_test.c index a60e625..a712c51 100644 --- a/frtos/line_sensor/line_sensor_test.c +++ b/frtos/line_sensor/line_sensor_test.c @@ -1,88 +1,27 @@ -#include "line_sensor.h" +#include "line_sensor_init.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() -{ - // 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; - xTaskCreate(monitor_left_sensor_task, - "Monitor Left Sensor Task", - configMINIMAL_STACK_SIZE, - NULL, - 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", - 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(); -} int -main (void) +main(void) { stdio_usb_init(); -// sleep_ms(2000); + obs_t obs; + + car_struct_t car_struct = {.obs = &obs}; + + sleep_ms(2000); + printf("Test started!\n"); - line_sensor_setup(); - initialize_car_state(); + line_sensor_init(&car_struct); + printf("Line sensor initialized!\n"); - launch(); + line_tasks_init(&car_struct); + + vTaskStartScheduler(); return (0); } \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index e21ca74..b2b1ba5 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -340,7 +340,7 @@ void updateDirection() { // Temperature in degrees Celsius // printf("Temperature: %d\n", temperature[0]); - print_orientation_data(); +// print_orientation_data(); // printf("Direction: "); diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 8107d29..6fce7f0 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -19,8 +19,6 @@ * the same function. * if the motor direction is not set, it will not move. * @param direction The direction of the left and right wheels - * @param left_speed The speed of the left motor, from 0.0 to 1.0 - * @param right_speed The speed of the right motor, from 0.0 to 1.0 */ void set_wheel_direction(uint32_t direction) @@ -43,6 +41,13 @@ revert_wheel_direction() gpio_set_mask(reverted_direction & DIRECTION_MASK); } +/*! + * @brief Check if the current direction is within the range of the target + * @param current_direction current yaw + * @param target_direction target yaw + * @param range acceptable range + * @return true if the current direction is within the range of the target + */ bool check_direction(float current_direction, float target_direction, float range) { @@ -64,28 +69,19 @@ check_direction(float current_direction, float target_direction, float range) return false; } +/*! + * @brief Spin the car to a certain yaw + * @param target_yaw The target yaw to spin to + * @param pp_car_struct The car struct pointer + */ void -spin_to_yaw(float target_yaw, car_struct_t *car_struct) +spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct) { - updateDirection(); - float initial_yaw = g_direction.yaw; + set_wheel_direction(direction); - // if it will to turn more than 180 degrees, turn the other way - if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) - || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) - { - set_wheel_direction(DIRECTION_RIGHT); - } - else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) - || ((target_yaw < initial_yaw) - && (initial_yaw - target_yaw < 180.f))) - { - set_wheel_direction(DIRECTION_LEFT); - } + set_wheel_speed_synced(80u, pp_car_struct); - set_wheel_speed_synced(80u, car_struct); - - car_struct->p_pid->use_pid = false; + pp_car_struct->p_pid->use_pid = false; for (;;) { @@ -93,13 +89,31 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) if (check_direction(g_direction.yaw, target_yaw, 1)) { set_wheel_direction(DIRECTION_MASK); - set_wheel_speed_synced(0u, car_struct); + set_wheel_speed_synced(0u, pp_car_struct); break; } } - car_struct->p_pid->use_pid = true; + pp_car_struct->p_pid->use_pid = true; vTaskDelay(pdMS_TO_TICKS(50)); } +void +spin_right(float degree, car_struct_t *pp_car_struct) +{ + updateDirection(); + float initial_yaw = g_direction.yaw; + float target_yaw = adjust_yaw(initial_yaw + degree); + spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct); +} + +void +spin_left(float degree, car_struct_t *pp_car_struct) +{ + updateDirection(); + float initial_yaw = g_direction.yaw; + float target_yaw = adjust_yaw(initial_yaw - degree); + spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct); +} + #endif /* MOTOR_DIRECTION_H */ \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 3b7aad3..c4e5c20 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -17,8 +17,19 @@ #include "task.h" #include "semphr.h" -#include "motor_config.h" +#include "motor_speed.h" +#include "motor_direction.h" +#include "motor_pid.h" +#include "car_config.h" + +/*! + * @brief Initialize the motor + * @param car_struct The car_struct. Need to have the following fields:\n + * - p_left_motor\n + * - p_right_motor\n + * - p_pid + */ void motor_init(car_struct_t *car_struct) { @@ -85,4 +96,43 @@ motor_init(car_struct_t *car_struct) pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true); } +/*! + * @brief init the tasks for the motor + * @param pp_car_struct The car struct + * @param p_isr_handler The isr handler + */ +void +motor_tasks_init(car_struct_t *pp_car_struct, void *p_isr_handler) +{ + // Left wheel + // + TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_wheel_speed_task, + "monitor_left_wheel_speed_task", + configMINIMAL_STACK_SIZE, + (void *)pp_car_struct->p_left_motor, + PRIO, + &h_monitor_left_wheel_speed_task_handle); + + // Right wheel + // + TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_wheel_speed_task, + "monitor_wheel_speed_task", + configMINIMAL_STACK_SIZE, + (void *)pp_car_struct->p_right_motor, + PRIO, + &h_monitor_right_wheel_speed_task_handle); + + // isr to detect right motor slot + gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, p_isr_handler); + + // isr to detect left motor slot + gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(SPEED_PIN_LEFT, p_isr_handler); + + irq_set_enabled(IO_IRQ_BANK0, true); +} + #endif /* MOTOR_INIT_H */ \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index dd858e6..5cce816 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -8,14 +8,11 @@ #ifndef MOTOR_PID_H #define MOTOR_PID_H -// #include "magnetometer_init.h" - /*! * @brief Compute the control signal using PID controller - * @param target_speed The target speed of the wheel - * @param current_speed The current speed of the wheel * @param integral The integral term of the PID controller * @param prev_error The previous error of the PID controller + * @param car_struct The car_struct pointer * @return The control signal */ float @@ -38,10 +35,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) return control_signal; } +/*! + * @brief Repeating timer handler for the PID controller + * @param ppp_timer The repeating timer + * @return true + */ bool -repeating_pid_handler(struct repeating_timer *t) +repeating_pid_handler(struct repeating_timer *ppp_timer) { - car_struct_t *car_strut = (car_struct_t *)t->user_data; + car_struct_t *car_strut = (car_struct_t *)ppp_timer->user_data; static float integral = 0.0f; static float prev_error = 0.0f; diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 881bb24..8c0ed5d 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -38,14 +38,14 @@ h_wheel_sensor_isr_handler(void) /*! * @brief Task to monitor and control the speed of the wheel - * @param pvParameters motor_speed_t struct pointer + * @param ppp_motor motor_speed_t struct pointer * @see motor_speed_t */ void -monitor_wheel_speed_task(void *pvParameters) +monitor_wheel_speed_task(void *ppp_motor) { volatile motor_t *p_motor = NULL; - p_motor = (motor_t *)pvParameters; + p_motor = (motor_t *)ppp_motor; uint64_t curr_time = 0u; uint64_t prev_time = 0u; @@ -68,6 +68,8 @@ monitor_wheel_speed_task(void *pvParameters) = (float) (1021017.61242f / elapsed_time); p_motor->speed.distance_cm += 1.02101761242f; + + printf("speed\n"); } else { @@ -76,30 +78,35 @@ monitor_wheel_speed_task(void *pvParameters) } } -void -set_wheel_speed(uint32_t pwm_level, motor_t * motor) -{ - motor->pwm.level = pwm_level; - - pwm_set_chan_level(motor->pwm.slice_num, - motor->pwm.channel, - motor->pwm.level); -} - /*! * @brief Set the speed of the wheels * @param pwm_level The pwm_level of the wheels, from 0 to 99 + * @param p_motor The motor to set the speed */ void -set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *car_strut) +set_wheel_speed(uint32_t pwm_level, motor_t *p_motor) { if (pwm_level > MAX_PWM_LEVEL) { pwm_level = MAX_PWM_LEVEL; } - set_wheel_speed(pwm_level, car_strut->p_left_motor); - set_wheel_speed(pwm_level, car_strut->p_right_motor); + p_motor->pwm.level = pwm_level; + + pwm_set_chan_level( + p_motor->pwm.slice_num, p_motor->pwm.channel, p_motor->pwm.level); +} + +/*! + * @brief Set the speed of the wheels + * @param pwm_level The pwm_level of the wheels, from 0 to 99 + * @param pp_car_strut The car struct pointer + */ +void +set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut) +{ + set_wheel_speed(pwm_level, pp_car_strut->p_left_motor); + set_wheel_speed(pwm_level, pp_car_strut->p_right_motor); } ///*! diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 7a45243..9e2d8f7 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -1,8 +1,5 @@ - -#include "motor_speed.h" -#include "motor_direction.h" -#include "motor_pid.h" +#include "motor_init.h" void motor_control_task(void *params) @@ -22,48 +19,7 @@ motor_control_task(void *params) } } -void -launch(car_struct_t *car_struct, void *isr_handler) -{ - // Left wheel - // - TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_wheel_speed_task, - "monitor_left_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *)car_struct->p_left_motor, - WHEEL_SPEED_PRIO, - &h_monitor_left_wheel_speed_task_handle); - // Right wheel - // - TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_wheel_speed_task, - "monitor_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *)car_struct->p_right_motor, - WHEEL_SPEED_PRIO, - &h_monitor_right_wheel_speed_task_handle); - - // control task - TaskHandle_t h_motor_turning_task_handle = NULL; - xTaskCreate(motor_control_task, - "motor_turning_task", - configMINIMAL_STACK_SIZE, - (void *)car_struct, - WHEEL_CONTROL_PRIO, - &h_motor_turning_task_handle); - - // isr to detect right motor slot - gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, isr_handler); - - // isr to detect left motor slot - gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(SPEED_PIN_LEFT, isr_handler); - - irq_set_enabled(IO_IRQ_BANK0, true); -} int main(void) @@ -73,17 +29,26 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_t g_motor_right; - motor_t g_motor_left; - motor_pid_t g_pid; + motor_t motor_right; + motor_t motor_left; + motor_pid_t pid; - car_struct_t car_struct = { .p_right_motor = &g_motor_right, - .p_left_motor = &g_motor_left, - .p_pid = &g_pid }; + car_struct_t car_struct = { .p_right_motor = &motor_right, + .p_left_motor = &motor_left, + .p_pid = &pid }; motor_init(&car_struct); - launch(&car_struct, &h_wheel_sensor_isr_handler); + motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler); + + // control task + TaskHandle_t h_motor_turning_task_handle = NULL; + xTaskCreate(motor_control_task, + "motor_turning_task", + configMINIMAL_STACK_SIZE, + (void *)&car_struct, + WHEEL_CONTROL_PRIO, + &h_motor_turning_task_handle); // PID timer struct repeating_timer pid_timer; diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 2d0da53..7e256b6 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -1,147 +1,25 @@ -/** - * @brief Program to read onboard temperature sensor and print out the average - */ - -#include - -// pico sdk libraries -#include "pico/cyw43_arch.h" -#include "pico/stdlib.h" - -// FreeRTOS libraries -#include "FreeRTOS.h" -#include "task.h" - -#include "motor_speed.h" -#include "motor_direction.h" -#include "motor_pid.h" - -// #include "line_sensor.h" +#include "line_sensor_init.h" #include "ultrasonic_sensor.h" - -// #define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -// #define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -// #define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -// -// #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL) - -#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL) - -/* Common Car State Structure (TODO: TBC)*/ -// static car_state_t g_car_state; - -static void -motor_control_task(__unused void *p_param) -{ - vTaskDelay(1000); - -// set_wheel_direction(DIRECTION_FORWARD); -// set_wheel_speed_synced(90); -// vTaskDelay(1000); - -// spin_to_yaw(300); -// -// set_wheel_direction(DIRECTION_FORWARD); -// set_wheel_speed_synced(90); -// vTaskDelay(1000); - - spin_to_yaw(65); - - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90); - vTaskDelay(10000); - -// set_wheel_direction(DIRECTION_MASK); - - for (;;); -} +#include "car_config.h" +#include "motor_init.h" void -launch() +motor_control_task(void *params) { - // // 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); + car_struct_t *car_struct = (car_struct_t *)params; + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90u, car_struct); - // isr for ultrasonic + vTaskDelay(pdMS_TO_TICKS(10000)); - // isr to detect right motor slot - gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler); + revert_wheel_direction(); + set_wheel_speed_synced(90u, car_struct); - // isr to detect left motor slot - gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler); - - irq_set_enabled(IO_IRQ_BANK0, true); - - // // line sensor timer - // 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); - - // PID timer - struct repeating_timer pid_timer; - add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); - - // Line sensor - // TaskHandle_t h_monitor_left_sensor_task; - // xTaskCreate(monitor_left_sensor_task, - // "Monitor Left Sensor Task", - // configMINIMAL_STACK_SIZE, - // NULL, - // READ_LEFT_SENSOR_PRIO, - // &h_monitor_left_sensor_task); - - // Left wheel - // - TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_wheel_speed_task, - "monitor_left_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *)&g_motor_left, - WHEEL_SPEED_PRIO, - &h_monitor_left_wheel_speed_task_handle); - - // Right wheel - // - TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_wheel_speed_task, - "monitor_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *)&g_motor_right, - WHEEL_SPEED_PRIO, - &h_monitor_right_wheel_speed_task_handle); - - // control task - TaskHandle_t h_motor_turning_task_handle = NULL; - xTaskCreate(motor_control_task, - "motor_turning_task", - configMINIMAL_STACK_SIZE, - NULL, - WHEEL_CONTROL_PRIO, - &h_motor_turning_task_handle); - - // ultra - TaskHandle_t disttask; - xTaskCreate(distance_task, - "TestDistThread", - configMINIMAL_STACK_SIZE, - NULL, - 1, - &disttask); - - // magnetometer - // struct repeating_timer g_direction_timer; - // add_repeating_timer_ms(DIRECTION_READ_DELAY, - // h_direction_timer_handler, - // NULL, - // &g_direction_timer); - - vTaskStartScheduler(); + vTaskDelay(pdMS_TO_TICKS(10000)); + } } int @@ -149,26 +27,46 @@ main(void) { stdio_usb_init(); - sleep_ms(4000); - printf("Test started!\n"); + obs_t obs; - motor_init(); - printf("motor init"); + motor_t motor_right; + motor_t motor_left; + motor_pid_t pid; - magnetometer_init(); - printf("magnet init"); + car_struct_t car_struct = { .p_right_motor = &motor_right, + .p_left_motor = &motor_left, + .p_pid = &pid, + .obs = &obs}; - // line_sensor_setup(); + // ultra + ultrasonic_init(&car_struct); + ultrasonic_task_init(&car_struct); + printf("Ultrasonic sensor initialized!\n"); - init_ultrasonic(); - printf("ultraman init"); + // line + line_sensor_init(&car_struct); + line_tasks_init(&car_struct); + printf("Line sensor initialized!\n"); - // initialize_car_state(); // TODO: Could be common functionality, To - // confirm - // during Integration - launch(); + //motor + motor_init(&car_struct); + motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler); + printf("Motor initialized!\n"); + + // control task + TaskHandle_t h_motor_turning_task_handle = NULL; + xTaskCreate(motor_control_task, + "motor_turning_task", + configMINIMAL_STACK_SIZE, + (void *)&car_struct, + PRIO, + &h_motor_turning_task_handle); + + // PID timer + struct repeating_timer pid_timer; + add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer); + + vTaskStartScheduler(); return (0); -} - -/*** end of file ***/ +} \ No newline at end of file diff --git a/frtos/ultrasonic_sensor/ultrasonic_init.h b/frtos/ultrasonic_sensor/ultrasonic_init.h index d2db86d..b92a534 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_init.h +++ b/frtos/ultrasonic_sensor/ultrasonic_init.h @@ -9,11 +9,52 @@ #include #include "pico/stdlib.h" -#include "ultrasonic_sensor_config.h" +#include "car_config.h" +#include "ultrasonic_sensor.h" void -init_ultrasonic(void) +check_obstacle(void *pvParameters) { + while (true) + { // 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(); + + // 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); + + // 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 +ultrasonic_init(car_struct_t *car_struct) +{ + car_struct->obs->ultrasonic_detected = false; + // Set up the echo pin gpio_init(ECHO_PIN); gpio_set_dir(ECHO_PIN, GPIO_IN); @@ -23,4 +64,16 @@ init_ultrasonic(void) gpio_set_dir(TRIG_PIN, GPIO_OUT); } +void +ultrasonic_task_init(car_struct_t *car_struct) +{ + TaskHandle_t h_monitor_ultrasonic_task = NULL; + xTaskCreate(check_obstacle, + "read_ultrasonic_task", + configMINIMAL_STACK_SIZE, + (void *)car_struct->obs, + PRIO, + &h_monitor_ultrasonic_task); +} + #endif /* ULTRASONIC_INIT_H */ \ No newline at end of file diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.c b/frtos/ultrasonic_sensor/ultrasonic_sensor.c index c5e7f6f..9dc40ae 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.c +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.c @@ -4,32 +4,24 @@ #include #include "ultrasonic_sensor.h" +#include "car_config.h" -void -vLaunch(void) -{ - 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); - - TaskHandle_t disttask; - xTaskCreate(distance_task, - "TestDistThread", - configMINIMAL_STACK_SIZE, - NULL, - 1, - &disttask); - - vTaskStartScheduler(); -} int main(void) { stdio_init_all(); - init_ultrasonic(); + + obs_t obs; + + car_struct_t car_struct = { .obs = &obs }; + + ultrasonic_init(&car_struct); sleep_ms(1000); - vLaunch(); + + ultrasonic_task_init(&car_struct); + + vTaskStartScheduler(); return 0; } diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.h b/frtos/ultrasonic_sensor/ultrasonic_sensor.h index 014f1bc..157fcb2 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.h +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h @@ -1,95 +1,35 @@ /** -* @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" - -// volatile uint32_t start_time; -// volatile uint32_t end_time; -volatile bool echo_rising = false; +#include "car_config.h" float KalmanFilter(float U) { - static float R = 10; // noise convariance can be 10, higher better smooth - static float H = 1; // Measurement Map scalar - static float Q = 10; // initial estimated convariance - static float P = 0; // initial error covariance - static float U_hat = 0; // initial estimated state - static float K = 0; // initial Kalman gain + static float R = 10; // noise convariance can be 10, higher better smooth + static float H = 1; // Measurement Map scalar + static float Q = 10; // initial estimated convariance + static float P = 0; // initial error covariance + static float U_hat = 0; // initial estimated state + static float K = 0; // initial Kalman gain - // Predict - // - K = P * H / (H * P * H + R); // Update Kalman gain - U_hat = U_hat + K * (U - H * U_hat); // Update estimated state + // Predict + // + K = P * H / (H * P * H + R); // Update Kalman gain + U_hat = U_hat + K * (U - H * U_hat); // Update estimated state - // Update error covariance - // - P = (1 - K * H) * P + Q; + // Update error covariance + // + P = (1 - K * H) * P + Q; - return U_hat; + return U_hat; } -// void -// echo_handler() -// { -// if (gpio_get(ECHO_PIN)) -// { -// start_time = time_us_32(); -// echo_rising = true; -// } -// else -// { -// end_time = time_us_32(); -// echo_rising = false; -// } -// } - -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 */ \ No newline at end of file