From 546f340f52aca1e0c4dafefd3fb8ee9daee92f70 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Thu, 9 Nov 2023 14:02:58 +0800 Subject: [PATCH] everything working, somehow --- frtos/config/car_config.h | 19 +- frtos/config/motor_config.h | 25 ++- frtos/config/ultrasonic_sensor_config.h | 5 - frtos/line_sensor/line_sensor.h | 145 -------------- frtos/line_sensor/line_sensor_init.h | 19 +- frtos/line_sensor/line_sensor_test.c | 33 +--- frtos/motor/motor_init.h | 45 ++++- frtos/motor/motor_speed.h | 2 + frtos/motor/motor_test.c | 60 +----- frtos/rtos_car.c | 202 +++++--------------- frtos/ultrasonic_sensor/ultrasonic_init.h | 59 +++++- frtos/ultrasonic_sensor/ultrasonic_sensor.c | 30 ++- frtos/ultrasonic_sensor/ultrasonic_sensor.h | 115 ----------- 13 files changed, 218 insertions(+), 541 deletions(-) delete mode 100644 frtos/line_sensor/line_sensor.h diff --git a/frtos/config/car_config.h b/frtos/config/car_config.h index ed37ac3..6e1a9de 100644 --- a/frtos/config/car_config.h +++ b/frtos/config/car_config.h @@ -1,16 +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 -typedef struct s_obs_struct { +#define PRIO (tskIDLE_PRIORITY + 1UL) + +typedef struct s_obs_struct +{ bool line_detected; bool ultrasonic_detected; -} obs_t; +} obs_t; typedef struct { - obs_t *obs; + 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 +#endif // CAR_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 dda705d..2f38225 100644 --- a/frtos/config/ultrasonic_sensor_config.h +++ b/frtos/config/ultrasonic_sensor_config.h @@ -8,9 +8,4 @@ #define ULTRASONIC_SENSOR_READ_DELAY (100) -typedef struct -{ - bool obstacle_detected; -} ultrasonic_t; - #endif // ULTRASONIC_CONFIG_H diff --git a/frtos/line_sensor/line_sensor.h b/frtos/line_sensor/line_sensor.h deleted file mode 100644 index fb34808..0000000 --- a/frtos/line_sensor/line_sensor.h +++ /dev/null @@ -1,145 +0,0 @@ -/** - * @file line_sensor.h - * @brief Monitor the line sensor and update the car state accordingly - * @author Woon Jun Wei - */ - -#ifndef LINE_SENSOR_H -#define LINE_SENSOR_H - -#include "line_sensor_init.h" - -/** - * @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 *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); -// } -// } -//} - -/** - * @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; -// } -// } -//} - -#endif /* LINE_SENSOR_H */ \ 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 3a3bd8f..69b0b13 100644 --- a/frtos/line_sensor/line_sensor_init.h +++ b/frtos/line_sensor/line_sensor_init.h @@ -29,10 +29,9 @@ SemaphoreHandle_t g_left_sensor_sem = NULL; * This function will setup the Line Sensor by initializing it as an input */ static inline void -line_sensor_init() { -// obs_t obs = {0, 0}; -// -// p_car->obs = &obs; +line_sensor_init(car_struct_t *p_car_struct) +{ + p_car_struct->obs->line_detected = false; g_left_sensor_sem = xSemaphoreCreateBinary(); @@ -76,5 +75,17 @@ monitor_left_sensor_task(void *pvParameters) { } } +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 1ecc429..a712c51 100644 --- a/frtos/line_sensor/line_sensor_test.c +++ b/frtos/line_sensor/line_sensor_test.c @@ -1,50 +1,25 @@ #include "line_sensor_init.h" -#include "ultrasonic_sensor.h" #include "car_config.h" -#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) - -void -launch(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, - READ_LEFT_SENSOR_PRIO, - &h_monitor_left_sensor_task); - - TaskHandle_t h_monitor_ultrasonic_task = NULL; - xTaskCreate(check_obstacle, - "read_ultrasonic_task", - configMINIMAL_STACK_SIZE, - (void *)car_struct->obs, - READ_LEFT_SENSOR_PRIO, - &h_monitor_ultrasonic_task); -} int main(void) { stdio_usb_init(); - obs_t obs = { 0, 0 }; + obs_t obs; - car_struct_t car_struct = { .obs = &obs }; + car_struct_t car_struct = {.obs = &obs}; sleep_ms(2000); printf("Test started!\n"); - line_sensor_init(); + line_sensor_init(&car_struct); printf("Line sensor initialized!\n"); - init_ultrasonic(); - printf("Ultrasonic sensor initialized!\n"); - - launch(&car_struct); + line_tasks_init(&car_struct); vTaskStartScheduler(); diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index ababd87..c4e5c20 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -17,7 +17,11 @@ #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 @@ -92,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_speed.h b/frtos/motor/motor_speed.h index e19e19a..8c0ed5d 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -68,6 +68,8 @@ monitor_wheel_speed_task(void *ppp_motor) = (float) (1021017.61242f / elapsed_time); p_motor->speed.distance_cm += 1.02101761242f; + + printf("speed\n"); } else { diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index ab1e096..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,53 +19,7 @@ motor_control_task(void *params) } } -/*! - * @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, - 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 *)pp_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 *)pp_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, 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); -} int main(void) @@ -90,6 +41,15 @@ main(void) 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; add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &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 f8ee234..b92a534 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_init.h +++ b/frtos/ultrasonic_sensor/ultrasonic_init.h @@ -9,13 +9,52 @@ #include #include "pico/stdlib.h" -#include "ultrasonic_sensor_config.h" - -ultrasonic_t ultrasonic_sensor = { .obstacle_detected = false }; +#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); @@ -25,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 830a48b..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(check_obstacle, - "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 e7f9b0b..157fcb2 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.h +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h @@ -8,13 +8,8 @@ #define ULTRASONIC_SENSOR_H #include "ultrasonic_init.h" -//#include "motor_speed.h" #include "car_config.h" -// volatile uint32_t start_time; -// volatile uint32_t end_time; -// volatile bool echo_rising = false; - float KalmanFilter(float U) { @@ -37,114 +32,4 @@ KalmanFilter(float U) 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 -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 -//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 */ \ No newline at end of file