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

Merge remote-tracking branch 'origin/main'

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

9
frtos/car/car.h
View File

@ -7,14 +7,5 @@
#include "car_init.h" #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 */

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

13
frtos/config/ultrasonic_sensor_config.h
View File

@ -3,9 +3,14 @@
/* 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 typedef struct
{
bool obstacle_detected;
} ultrasonic_t;
#endif // ULTRASONIC_CONFIG_H

2
frtos/line_sensor/CMakeLists.txt
View File

@ -12,6 +12,8 @@ target_link_libraries(
target_include_directories(line_sensor_test 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)

175
frtos/line_sensor/line_sensor.h
View File

@ -4,58 +4,11 @@
* @author Woon Jun Wei * @author Woon Jun Wei
*/ */
#ifndef LINE_SENSOR_H
#define LINE_SENSOR_H
#include "line_sensor_init.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 * @brief Monitor the right sensor
* *
@ -85,59 +38,25 @@ monitor_left_sensor_task(__unused void *params) {
// } // }
//} //}
void //void
monitor_right_sensor_task(void *params) { //monitor_right_sensor_task(void *pvParameters) {
for (;;) { //
if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) { // volatile
// Check the flag or receive the message // for (;;) {
printf("right sensor triggered\n"); // if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) {
// Get Current State // // Check the flag or receive the message
state_t state = gpio_get(RIGHT_SENSOR_PIN); // 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 * @brief Monitor the direction and Oritentation of the car
* *
@ -146,29 +65,29 @@ void monitor_barcode_sensor_task(void *params) {
* *
* @param params * @param params
*/ */
void //void
monitor_direction_task(__unused void *params) { //monitor_direction_task(__unused void *params) {
state_t left_state; // state_t left_state;
state_t right_state; // state_t right_state;
state_t barcode_state; // state_t barcode_state;
//
for (;;) // for (;;)
{ // {
// Receive from Buffer // // Receive from Buffer
xMessageBufferReceive(left_sensor_msg_buffer, // xMessageBufferReceive(left_sensor_msg_buffer,
&left_state, // &left_state,
sizeof(state_t), // sizeof(state_t),
portMAX_DELAY); // portMAX_DELAY);
//
xMessageBufferReceive(right_sensor_msg_buffer, // xMessageBufferReceive(right_sensor_msg_buffer,
&right_state, // &right_state,
sizeof(state_t), // sizeof(state_t),
portMAX_DELAY); // portMAX_DELAY);
//
xMessageBufferReceive(barcode_sensor_msg_buffer, // xMessageBufferReceive(barcode_sensor_msg_buffer,
&barcode_state, // &barcode_state,
sizeof(state_t), // sizeof(state_t),
portMAX_DELAY); // portMAX_DELAY);
// g_car_state.current_direction = (left_state << 1) | right_state; // g_car_state.current_direction = (left_state << 1) | right_state;
@ -220,5 +139,7 @@ monitor_direction_task(__unused void *params) {
// printf("Orientation: Error\n"); // printf("Orientation: Error\n");
// break; // break;
// } // }
} // }
} //}
#endif /* LINE_SENSOR_H */

173
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,19 @@ 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() {
g_left_sensor_sem = xSemaphoreCreateBinary(); // obs_t obs = {0, 0};
g_right_sensor_sem = xSemaphoreCreateBinary(); //
g_barcode_sensor_sem = xSemaphoreCreateBinary(); // 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 // 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 +58,23 @@ 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);
} }
#endif /* LINE_SENSOR_INIT_H */ #endif /* LINE_SENSOR_INIT_H */

94
frtos/line_sensor/line_sensor_test.c
View File

@ -1,88 +1,52 @@
#include "line_sensor.h" #include "line_sensor_init.h"
#include "ultrasonic_sensor.h"
#include "car_config.h"
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) #define READ_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 void
launch() launch(car_struct_t *car_struct)
{ {
// isr to detect left line sensor TaskHandle_t h_monitor_left_sensor_task = NULL;
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, xTaskCreate(monitor_left_sensor_task,
"Monitor Left Sensor Task", "read_left_sensor_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *)car_struct->obs,
READ_LEFT_SENSOR_PRIO, READ_LEFT_SENSOR_PRIO,
&h_monitor_left_sensor_task); &h_monitor_left_sensor_task);
TaskHandle_t h_monitor_right_sensor_task; TaskHandle_t h_monitor_ultrasonic_task = NULL;
xTaskCreate(monitor_right_sensor_task, xTaskCreate(check_obstacle,
"Monitor Right Sensor Task", "read_ultrasonic_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *)car_struct->obs,
READ_RIGHT_SENSOR_PRIO, READ_LEFT_SENSOR_PRIO,
&h_monitor_right_sensor_task); &h_monitor_ultrasonic_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 = { 0, 0 };
car_struct_t car_struct = { .obs = &obs };
sleep_ms(2000);
printf("Test started!\n"); printf("Test started!\n");
line_sensor_setup(); line_sensor_init();
initialize_car_state(); printf("Line sensor initialized!\n");
launch(); init_ultrasonic();
printf("Ultrasonic sensor initialized!\n");
launch(&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: ");

2
frtos/ultrasonic_sensor/ultrasonic_init.h
View File

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

6
frtos/ultrasonic_sensor/ultrasonic_sensor.c
View File

@ -8,12 +8,12 @@
void void
vLaunch(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; TaskHandle_t disttask;
xTaskCreate(distance_task, xTaskCreate(check_obstacle,
"TestDistThread", "TestDistThread",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, NULL,

159
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -1,39 +1,40 @@
/** /**
* @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 "motor_speed.h"
#include "car_config.h"
// volatile uint32_t start_time; // volatile uint32_t start_time;
// volatile uint32_t end_time; // volatile uint32_t end_time;
volatile bool echo_rising = false; // 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 // void
@ -52,44 +53,98 @@ KalmanFilter(float U)
// } // }
void void
distance_task(__unused void *params) check_obstacle(void *pvParameters)
{ {
while (true) while (true)
{ { // Put trigger pin high for 10us
vTaskDelay(1000); gpio_put(TRIG_PIN, 1);
sleep_us(10);
gpio_put(TRIG_PIN, 0);
gpio_put(TRIG_PIN, 1); // Wait for echo pin to go high
sleep_us(10); while (gpio_get(ECHO_PIN) == 0)
gpio_put(TRIG_PIN, 0); tight_loop_contents();
while (gpio_get(ECHO_PIN) == 0) // Measure the pulse width (time taken for the echo to return)
tight_loop_contents(); 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 end_time = time_us_32();
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) // Calculate the distance (in centimeters)
uint32_t pulse_duration = end_time - start_time; uint32_t pulse_duration = end_time - start_time;
float distance float distance
= (pulse_duration * 0.034 / 2); // Speed of sound in cm/us = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
printf("Distance: %.2f cm\n", distance); // printf("Distance: %.2f cm\n", distance);
// printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
if (distance < 7) // change value of obstacle_detected in ultrasonic_t struct
{ obs_t *ultrasonic_sensor = (obs_t *)pvParameters;
// set_wheel_direction(DIRECTION_MASK); ultrasonic_sensor->ultrasonic_detected = (distance < 7);
set_wheel_speed_synced(0u);
printf("Collision Imminent!\n"); printf("Distance: %.2f cm, Obstacle Detected: %d\n",
vTaskDelay(pdMS_TO_TICKS(3000)); distance,
spin_to_yaw(350); ultrasonic_sensor->ultrasonic_detected);
set_wheel_direction(DIRECTION_FORWARD); vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
set_wheel_speed_synced(90u); }
}
start_time, end_time = 0;
}
} }
//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 */ #endif /* ULTRASONIC_SENSOR_H */