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 */ /* 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/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,

157
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

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