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 branch 'L4ncelot-R:main' into main

This commit is contained in:
OldManSteve 2023-11-09 15:00:48 +08:00 committed by GitHub
parent 21ef7e8892 6ac564ee04
commit 41a21ed506
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 361 additions and 862 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" #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 */ #endif /* CAR_H */

28
frtos/car/car_init.h
View File

@ -12,33 +12,5 @@
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.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 */ #endif /* CAR_INIT_H */

27
frtos/config/car_config.h Normal file
View File

@ -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

6
frtos/config/line_sensor_config.h
View File

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

25
frtos/config/motor_config.h
View File

@ -34,10 +34,6 @@
#define MAX_PWM_LEVEL 99U #define MAX_PWM_LEVEL 99U
#define MIN_PWM_LEVEL 0U #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 * @brief Structure for the motor speed parameters
* @param current_speed_cms Current speed in cm/s * @param current_speed_cms Current speed in cm/s
@ -47,6 +43,7 @@ typedef struct
{ {
float current_cms; float current_cms;
float distance_cm; float distance_cm;
} motor_speed_t; } motor_speed_t;
/*! /*!
@ -67,6 +64,7 @@ typedef struct
* @param pid_kp Proportional gain * @param pid_kp Proportional gain
* @param pid_ki Integral gain * @param pid_ki Integral gain
* @param pid_kd Derivative gain * @param pid_kd Derivative gain
* @param use_pid Flag to use PID or not
*/ */
typedef struct typedef struct
{ {
@ -79,9 +77,9 @@ typedef struct
/*! /*!
* @brief Structure for the motor parameters * @brief Structure for the motor parameters
* @param speed Motor speed parameters * @param speed Motor speed parameters
* @param sem Semaphore for the motor speed
* @param pwm Motor PWM parameters * @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 typedef struct
{ {
@ -102,12 +100,13 @@ typedef struct
SemaphoreHandle_t g_left_sem; SemaphoreHandle_t g_left_sem;
SemaphoreHandle_t g_right_sem; SemaphoreHandle_t g_right_sem;
typedef struct // for testing
{ //typedef struct
motor_t * p_left_motor; //{
motor_t * p_right_motor; // motor_t * p_left_motor;
motor_pid_t * p_pid; // motor_t * p_right_motor;
// motor_pid_t * p_pid;
} car_struct_t; //
//} car_struct_t;
#endif /* MOTOR_CONFIG_H */ #endif /* MOTOR_CONFIG_H */

8
frtos/config/ultrasonic_sensor_config.h
View File

@ -3,9 +3,9 @@
/* ADC Configuration */ /* ADC Configuration */
#define TRIG_PIN ( 2 ) #define TRIG_PIN (2)
#define ECHO_PIN ( 3 ) #define ECHO_PIN (3)
#define ULTRASONIC_SENSOR_READ_DELAY ( 500 ) #define ULTRASONIC_SENSOR_READ_DELAY (100)
#endif //ULTRASONIC_CONFIG_H #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 target_include_directories(line_sensor_test
PRIVATE ../config PRIVATE ../config
../car
../ultrasonic_sensor
) )
pico_enable_stdio_usb(line_sensor_test 1) pico_enable_stdio_usb(line_sensor_test 1)

224
frtos/line_sensor/line_sensor.h
View File

@ -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;
// }
}
}

184
frtos/line_sensor/line_sensor_init.h
View File

@ -18,64 +18,10 @@
#include "semphr.h" #include "semphr.h"
#include "line_sensor_config.h" #include "line_sensor_config.h"
#include "car_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;
// Semaphore // Semaphore
SemaphoreHandle_t g_left_sensor_sem = NULL; 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 * @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 * This function will setup the Line Sensor by initializing it as an input
*/ */
static inline void static inline void
line_sensor_setup() { line_sensor_init(car_struct_t *p_car_struct)
g_left_sensor_sem = xSemaphoreCreateBinary(); {
g_right_sensor_sem = xSemaphoreCreateBinary(); p_car_struct->obs->line_detected = false;
g_barcode_sensor_sem = xSemaphoreCreateBinary();
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 // Initialise 3 GPIO pins and set them to input
gpio_init_mask(mask); gpio_init_mask(mask);
gpio_set_dir_in_masked(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; 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; void
xSemaphoreGiveFromISR(g_right_sensor_sem, monitor_left_sensor_task(void *pvParameters) {
&xHigherPriorityTaskWoken); volatile obs_t *p_obs = NULL;
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); p_obs = (obs_t *) pvParameters;
return true; for (;;)
}
/**
* @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)
{ {
if ((currentTicks - lastEdgeTimeLeft) >= // if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
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);
// Set the flag to notify the task // Set the flag to notify the task
right_sensor_triggered = pdTRUE; p_obs->line_detected = gpio_get(LEFT_SENSOR_PIN);
xSemaphoreGiveFromISR(g_right_sensor_sem, printf("Left Sensor: %d\n", p_obs->line_detected);
&xHigherPriorityTaskWoken); 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);
} }
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 */ #endif /* LINE_SENSOR_INIT_H */

89
frtos/line_sensor/line_sensor_test.c
View File

@ -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 int
main (void) main(void)
{ {
stdio_usb_init(); stdio_usb_init();
// sleep_ms(2000); obs_t obs;
car_struct_t car_struct = {.obs = &obs};
sleep_ms(2000);
printf("Test started!\n"); printf("Test started!\n");
line_sensor_setup(); line_sensor_init(&car_struct);
initialize_car_state(); printf("Line sensor initialized!\n");
launch(); line_tasks_init(&car_struct);
vTaskStartScheduler();
return (0); return (0);
} }

2
frtos/magnetometer/magnetometer_direction.h
View File

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

58
frtos/motor/motor_direction.h
View File

@ -19,8 +19,6 @@
* the same function. * the same function.
* if the motor direction is not set, it will not move. * if the motor direction is not set, it will not move.
* @param direction The direction of the left and right wheels * @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 void
set_wheel_direction(uint32_t direction) set_wheel_direction(uint32_t direction)
@ -43,6 +41,13 @@ revert_wheel_direction()
gpio_set_mask(reverted_direction & DIRECTION_MASK); 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 bool
check_direction(float current_direction, float target_direction, float range) 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; 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 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(); set_wheel_direction(direction);
float initial_yaw = g_direction.yaw;
// if it will to turn more than 180 degrees, turn the other way set_wheel_speed_synced(80u, pp_car_struct);
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, car_struct); pp_car_struct->p_pid->use_pid = false;
car_struct->p_pid->use_pid = false;
for (;;) 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)) if (check_direction(g_direction.yaw, target_yaw, 1))
{ {
set_wheel_direction(DIRECTION_MASK); set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, car_struct); set_wheel_speed_synced(0u, pp_car_struct);
break; break;
} }
} }
car_struct->p_pid->use_pid = true; pp_car_struct->p_pid->use_pid = true;
vTaskDelay(pdMS_TO_TICKS(50)); 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 */ #endif /* MOTOR_DIRECTION_H */

52
frtos/motor/motor_init.h
View File

@ -17,8 +17,19 @@
#include "task.h" #include "task.h"
#include "semphr.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 void
motor_init(car_struct_t *car_struct) 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); 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 */ #endif /* MOTOR_INIT_H */

14
frtos/motor/motor_pid.h
View File

@ -8,14 +8,11 @@
#ifndef MOTOR_PID_H #ifndef MOTOR_PID_H
#define MOTOR_PID_H #define MOTOR_PID_H
// #include "magnetometer_init.h"
/*! /*!
* @brief Compute the control signal using PID controller * @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 integral The integral term of the PID controller
* @param prev_error The previous error 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 * @return The control signal
*/ */
float float
@ -38,10 +35,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
return control_signal; return control_signal;
} }
/*!
* @brief Repeating timer handler for the PID controller
* @param ppp_timer The repeating timer
* @return true
*/
bool 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 integral = 0.0f;
static float prev_error = 0.0f; static float prev_error = 0.0f;

39
frtos/motor/motor_speed.h
View File

@ -38,14 +38,14 @@ h_wheel_sensor_isr_handler(void)
/*! /*!
* @brief Task to monitor and control the speed of the wheel * @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 * @see motor_speed_t
*/ */
void void
monitor_wheel_speed_task(void *pvParameters) monitor_wheel_speed_task(void *ppp_motor)
{ {
volatile motor_t *p_motor = NULL; volatile motor_t *p_motor = NULL;
p_motor = (motor_t *)pvParameters; p_motor = (motor_t *)ppp_motor;
uint64_t curr_time = 0u; uint64_t curr_time = 0u;
uint64_t prev_time = 0u; uint64_t prev_time = 0u;
@ -68,6 +68,8 @@ monitor_wheel_speed_task(void *pvParameters)
= (float) (1021017.61242f / elapsed_time); = (float) (1021017.61242f / elapsed_time);
p_motor->speed.distance_cm += 1.02101761242f; p_motor->speed.distance_cm += 1.02101761242f;
printf("speed\n");
} }
else 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 * @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 99 * @param pwm_level The pwm_level of the wheels, from 0 to 99
* @param p_motor The motor to set the speed
*/ */
void 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) if (pwm_level > MAX_PWM_LEVEL)
{ {
pwm_level = MAX_PWM_LEVEL; pwm_level = MAX_PWM_LEVEL;
} }
set_wheel_speed(pwm_level, car_strut->p_left_motor); p_motor->pwm.level = pwm_level;
set_wheel_speed(pwm_level, car_strut->p_right_motor);
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);
} }
///*! ///*!

69
frtos/motor/motor_test.c
View File

@ -1,8 +1,5 @@
#include "motor_init.h"
#include "motor_speed.h"
#include "motor_direction.h"
#include "motor_pid.h"
void void
motor_control_task(void *params) 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 int
main(void) main(void)
@ -73,17 +29,26 @@ main(void)
sleep_ms(4000); sleep_ms(4000);
printf("Test started!\n"); printf("Test started!\n");
motor_t g_motor_right; motor_t motor_right;
motor_t g_motor_left; motor_t motor_left;
motor_pid_t g_pid; motor_pid_t pid;
car_struct_t car_struct = { .p_right_motor = &g_motor_right, car_struct_t car_struct = { .p_right_motor = &motor_right,
.p_left_motor = &g_motor_left, .p_left_motor = &motor_left,
.p_pid = &g_pid }; .p_pid = &pid };
motor_init(&car_struct); 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 // PID timer
struct repeating_timer pid_timer; struct repeating_timer pid_timer;

202
frtos/rtos_car.c
View File

@ -1,147 +1,25 @@
/**
* @brief Program to read onboard temperature sensor and print out the average
*/
#include <stdio.h>
// 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" #include "ultrasonic_sensor.h"
#include "car_config.h"
// #define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) #include "motor_init.h"
// #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 (;;);
}
void void
launch() motor_control_task(void *params)
{ {
// // isr to detect left line sensor car_struct_t *car_struct = (car_struct_t *)params;
// gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); for (;;)
// gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler); {
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 revert_wheel_direction();
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); set_wheel_speed_synced(90u, car_struct);
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
// isr to detect left motor slot vTaskDelay(pdMS_TO_TICKS(10000));
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();
} }
int int
@ -149,26 +27,46 @@ main(void)
{ {
stdio_usb_init(); stdio_usb_init();
sleep_ms(4000); obs_t obs;
printf("Test started!\n");
motor_init(); motor_t motor_right;
printf("motor init"); motor_t motor_left;
motor_pid_t pid;
magnetometer_init(); car_struct_t car_struct = { .p_right_motor = &motor_right,
printf("magnet init"); .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(); // line
printf("ultraman init"); line_sensor_init(&car_struct);
line_tasks_init(&car_struct);
printf("Line sensor initialized!\n");
// initialize_car_state(); // TODO: Could be common functionality, To //motor
// confirm motor_init(&car_struct);
// during Integration motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler);
launch(); 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); return (0);
} }
/*** end of file ***/

57
frtos/ultrasonic_sensor/ultrasonic_init.h
View File

@ -9,11 +9,52 @@
#include <stdio.h> #include <stdio.h>
#include "pico/stdlib.h" #include "pico/stdlib.h"
#include "ultrasonic_sensor_config.h" #include "car_config.h"
#include "ultrasonic_sensor.h"
void 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 // Set up the echo pin
gpio_init(ECHO_PIN); gpio_init(ECHO_PIN);
gpio_set_dir(ECHO_PIN, GPIO_IN); gpio_set_dir(ECHO_PIN, GPIO_IN);
@ -23,4 +64,16 @@ init_ultrasonic(void)
gpio_set_dir(TRIG_PIN, GPIO_OUT); 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 */ #endif /* ULTRASONIC_INIT_H */

30
frtos/ultrasonic_sensor/ultrasonic_sensor.c
View File

@ -4,32 +4,24 @@
#include <stdio.h> #include <stdio.h>
#include "ultrasonic_sensor.h" #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 int
main(void) main(void)
{ {
stdio_init_all(); stdio_init_all();
init_ultrasonic();
obs_t obs;
car_struct_t car_struct = { .obs = &obs };
ultrasonic_init(&car_struct);
sleep_ms(1000); sleep_ms(1000);
vLaunch();
ultrasonic_task_init(&car_struct);
vTaskStartScheduler();
return 0; return 0;
} }

98
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -1,95 +1,35 @@
/** /**
* @file ultrasonic_sensor.h * @file ultrasonic_sensor.h
* @brief Monitor the distance between the car and the wall * @brief Monitor the distance between the car and the wall
* @author Poon Xiang Yuan * @author Poon Xiang Yuan
*/ */
#ifndef ULTRASONIC_SENSOR_H #ifndef ULTRASONIC_SENSOR_H
#define ULTRASONIC_SENSOR_H #define ULTRASONIC_SENSOR_H
#include "ultrasonic_init.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 float
KalmanFilter(float U) KalmanFilter(float U)
{ {
static float R = 10; // noise convariance can be 10, higher better smooth static float R = 10; // noise convariance can be 10, higher better smooth
static float H = 1; // Measurement Map scalar static float H = 1; // Measurement Map scalar
static float Q = 10; // initial estimated convariance static float Q = 10; // initial estimated convariance
static float P = 0; // initial error covariance static float P = 0; // initial error covariance
static float U_hat = 0; // initial estimated state static float U_hat = 0; // initial estimated state
static float K = 0; // initial Kalman gain static float K = 0; // initial Kalman gain
// Predict // Predict
// //
K = P * H / (H * P * H + R); // Update Kalman gain K = P * H / (H * P * H + R); // Update Kalman gain
U_hat = U_hat + K * (U - H * U_hat); // Update estimated state U_hat = U_hat + K * (U - H * U_hat); // Update estimated state
// Update error covariance // Update error covariance
// //
P = (1 - K * H) * P + Q; 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 */ #endif /* ULTRASONIC_SENSOR_H */