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/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/ultrasonic_sensor_config.h b/frtos/config/ultrasonic_sensor_config.h index 3bdcc22..dda705d 100644 --- a/frtos/config/ultrasonic_sensor_config.h +++ b/frtos/config/ultrasonic_sensor_config.h @@ -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 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 index 9aa8970..fb34808 100644 --- a/frtos/line_sensor/line_sensor.h +++ b/frtos/line_sensor/line_sensor.h @@ -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; // } - } -} \ No newline at end of file +// } +//} + +#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 b15447a..3a3bd8f 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,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); - } - 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); } + #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..1ecc429 100644 --- a/frtos/line_sensor/line_sensor_test.c +++ b/frtos/line_sensor/line_sensor_test.c @@ -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) +#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) 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); } \ 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/ultrasonic_sensor/ultrasonic_init.h b/frtos/ultrasonic_sensor/ultrasonic_init.h index d2db86d..f8ee234 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_init.h +++ b/frtos/ultrasonic_sensor/ultrasonic_init.h @@ -11,6 +11,8 @@ #include "pico/stdlib.h" #include "ultrasonic_sensor_config.h" +ultrasonic_t ultrasonic_sensor = { .obstacle_detected = false }; + void init_ultrasonic(void) { diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.c b/frtos/ultrasonic_sensor/ultrasonic_sensor.c index c5e7f6f..830a48b 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.c +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.c @@ -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, diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.h b/frtos/ultrasonic_sensor/ultrasonic_sensor.h index 014f1bc..e7f9b0b 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.h +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h @@ -1,39 +1,40 @@ /** -* @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) { - 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 @@ -52,44 +53,98 @@ KalmanFilter(float U) // } void -distance_task(__unused void *params) +check_obstacle(void *pvParameters) { - while (true) - { - vTaskDelay(1000); + while (true) + { // Put trigger pin high for 10us + gpio_put(TRIG_PIN, 1); + sleep_us(10); + gpio_put(TRIG_PIN, 0); - 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(); - 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(); - // 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(); + 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 + // 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)); + // 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 */ \ No newline at end of file