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

everything working, somehow

This commit is contained in:
Richie 2023-11-09 14:02:58 +08:00
parent 4761893cd9
commit 546f340f52
13 changed files with 218 additions and 541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
frtos/config/car_config.h
View File

@ -1,15 +1,26 @@
#include "motor_config.h"
#include "ultrasonic_sensor_config.h"
#include "magnetometer_config.h"
#include "line_sensor_config.h"
#ifndef CAR_CONFIG_H #ifndef CAR_CONFIG_H
#define CAR_CONFIG_H #define CAR_CONFIG_H
typedef struct s_obs_struct { #define PRIO (tskIDLE_PRIORITY + 1UL)
typedef struct s_obs_struct
{
bool line_detected; bool line_detected;
bool ultrasonic_detected; bool ultrasonic_detected;
} obs_t;
} obs_t;
typedef struct typedef struct
{ {
obs_t *obs; obs_t *obs;
motor_t *p_left_motor;
motor_t *p_right_motor;
motor_pid_t *p_pid;
} car_struct_t; } car_struct_t;

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

5
frtos/config/ultrasonic_sensor_config.h
View File

@ -8,9 +8,4 @@
#define ULTRASONIC_SENSOR_READ_DELAY (100) #define ULTRASONIC_SENSOR_READ_DELAY (100)
typedef struct
{
bool obstacle_detected;
} ultrasonic_t;
#endif // ULTRASONIC_CONFIG_H #endif // ULTRASONIC_CONFIG_H

145
frtos/line_sensor/line_sensor.h
View File

@ -1,145 +0,0 @@
/**
* @file line_sensor.h
* @brief Monitor the line sensor and update the car state accordingly
* @author Woon Jun Wei
*/
#ifndef LINE_SENSOR_H
#define LINE_SENSOR_H
#include "line_sensor_init.h"
/**
* @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 *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);
// }
// }
//}
/**
* @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;
// }
// }
//}
#endif /* LINE_SENSOR_H */

19
frtos/line_sensor/line_sensor_init.h
View File

@ -29,10 +29,9 @@ SemaphoreHandle_t g_left_sensor_sem = NULL;
* 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_init() { line_sensor_init(car_struct_t *p_car_struct)
// obs_t obs = {0, 0}; {
// p_car_struct->obs->line_detected = false;
// p_car->obs = &obs;
g_left_sensor_sem = xSemaphoreCreateBinary(); g_left_sensor_sem = xSemaphoreCreateBinary();
@ -76,5 +75,17 @@ monitor_left_sensor_task(void *pvParameters) {
} }
} }
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 */

31
frtos/line_sensor/line_sensor_test.c
View File

@ -1,36 +1,14 @@
#include "line_sensor_init.h" #include "line_sensor_init.h"
#include "ultrasonic_sensor.h"
#include "car_config.h" #include "car_config.h"
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
void
launch(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,
READ_LEFT_SENSOR_PRIO,
&h_monitor_left_sensor_task);
TaskHandle_t h_monitor_ultrasonic_task = NULL;
xTaskCreate(check_obstacle,
"read_ultrasonic_task",
configMINIMAL_STACK_SIZE,
(void *)car_struct->obs,
READ_LEFT_SENSOR_PRIO,
&h_monitor_ultrasonic_task);
}
int int
main(void) main(void)
{ {
stdio_usb_init(); stdio_usb_init();
obs_t obs = { 0, 0 }; obs_t obs;
car_struct_t car_struct = {.obs = &obs}; car_struct_t car_struct = {.obs = &obs};
@ -38,13 +16,10 @@ main(void)
printf("Test started!\n"); printf("Test started!\n");
line_sensor_init(); line_sensor_init(&car_struct);
printf("Line sensor initialized!\n"); printf("Line sensor initialized!\n");
init_ultrasonic(); line_tasks_init(&car_struct);
printf("Ultrasonic sensor initialized!\n");
launch(&car_struct);
vTaskStartScheduler(); vTaskStartScheduler();

45
frtos/motor/motor_init.h
View File

@ -17,7 +17,11 @@
#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 * @brief Initialize the motor
@ -92,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 */

2
frtos/motor/motor_speed.h
View File

@ -68,6 +68,8 @@ monitor_wheel_speed_task(void *ppp_motor)
= (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
{ {

60
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,53 +19,7 @@ motor_control_task(void *params)
} }
} }
/*!
* @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,
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 *)pp_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 *)pp_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, 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);
}
int int
main(void) main(void)
@ -90,6 +41,15 @@ main(void)
motor_tasks_init(&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;
add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer); add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);

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

59
frtos/ultrasonic_sensor/ultrasonic_init.h
View File

@ -9,13 +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"
ultrasonic_t ultrasonic_sensor = { .obstacle_detected = false };
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);
@ -25,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(check_obstacle,
"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;
} }

115
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -8,13 +8,8 @@
#define ULTRASONIC_SENSOR_H #define ULTRASONIC_SENSOR_H
#include "ultrasonic_init.h" #include "ultrasonic_init.h"
//#include "motor_speed.h"
#include "car_config.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)
{ {
@ -37,114 +32,4 @@ KalmanFilter(float U)
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
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
//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 */