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

ultrasonic update for integration

This commit is contained in:
xypoon 2023-11-09 12:29:02 +08:00
parent 8a89239362
commit 69e0e222dc
4 changed files with 98 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
frtos/config/ultrasonic_sensor_config.h
View File

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

2
frtos/ultrasonic_sensor/ultrasonic_init.h
View File

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

6
frtos/ultrasonic_sensor/ultrasonic_sensor.c
View File

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

157
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -1,8 +1,8 @@
/**
* @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
@ -12,28 +12,28 @@
// 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
@ -51,10 +51,11 @@ KalmanFilter(float U)
// }
// }
bool obstacle_detected = false;
void check_obstacle() {
// Put trigger pin high for 10us
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);
@ -75,62 +76,72 @@ void check_obstacle() {
float distance
= (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
printf("Distance: %.2f cm\n", distance);
// printf("Distance: %.2f cm\n", distance);
obstacle_detected = (distance < 7);
}
// change value of obstacle_detected in ultrasonic_t struct
ultrasonic_t *ultrasonic_sensor = (ultrasonic_t *)pvParameters;
ultrasonic_sensor->obstacle_detected = (distance < 7);
void distance_task_two(__unused void *params)
{
for(;;)
{
// Semaphore to release the task every 100ms
if (xSemaphoreTake(distance_semaphore, portMAX_DELAY))
{
check_obstacle();
}
}
printf("Distance: %.2f cm, Obstacle Detected: %d\n",
distance,
ultrasonic_sensor->obstacle_detected);
vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
}
}
void
distance_task(__unused void *params)
check_global(void *pvParameters)
{
while (true)
{
vTaskDelay(1000);
while (true)
{
ultrasonic_t *ultrasonic_sensor = (ultrasonic_t *)pvParameters;
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;
}
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 */