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

merged wheel monitor and control function to use struct for both wheels; removed set_wheel_speed function as it will be controlled by task directly; changed PID function to use more pointers

This commit is contained in:
Richie 2023-10-21 01:04:09 +08:00
parent c7950d3ec6
commit 9a9aac406e
4 changed files with 78 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
frtos/config/motor_config.h
View File

@ -32,4 +32,18 @@
#define MAX_SPEED 4900U #define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed #define MIN_SPEED 0U // To be changed
/*!
* @brief Structure for the motor speed
* @param side The side of the motor, 0 for left, 1 for right
* @param target_speed The target speed of the motor in cm/s
* @param pwm_level The current pwm level of the motor, in range [0, 5000]
*/
typedef struct {
float target_speed;
uint16_t pwm_level;
SemaphoreHandle_t * sem;
uint * p_slice_num;
uint channel;
} motor_speed_t;
#endif /* MOTOR_CONFIG_H */ #endif /* MOTOR_CONFIG_H */

143
frtos/motor/motor_speed.h
View File

@ -9,24 +9,6 @@
#include "motor_init.h" #include "motor_init.h"
/*!
* @brief Set the speed of the wheels; can use bitwise OR to set both
* @param speed in range [0, 5000]
* @param side 0 for left, 1 for right
*/
void
set_wheel_speed(float speed, uint8_t side)
{
if (side == 0U)
{
pwm_set_chan_level(g_slice_num_left, PWM_CHAN_A, (uint16_t)speed);
}
else
{
pwm_set_chan_level(g_slice_num_right, PWM_CHAN_B, (uint16_t)speed);
}
}
/*! /*!
* @brief Interrupt handler for the wheel sensor * @brief Interrupt handler for the wheel sensor
*/ */
@ -63,12 +45,12 @@ h_wheel_sensor_isr_handler(void)
* @return The control signal * @return The control signal
*/ */
float float
compute_pid(float target_speed, compute_pid(const volatile float * target_speed,
float current_speed, const float * current_speed,
float * integral, float * integral,
float * prev_error) float * prev_error)
{ {
float error = target_speed - current_speed; float error = *target_speed - *current_speed;
*integral += error; *integral += error;
float derivative = error - *prev_error; float derivative = error - *prev_error;
@ -81,47 +63,56 @@ compute_pid(float target_speed,
return control_signal; return control_signal;
} }
/*!
* @brief Task to monitor and control the speed of the wheel
* @param pvParameters motor_speed_t struct pointer
* @see motor_speed_t
*/
void void
monitor_left_wheel_speed_task(void *pvParameters) monitor_wheel_speed_task(void *pvParameters)
{ {
static volatile float speed_left = 0.f; volatile motor_speed_t *p_motor_speed = NULL;
static volatile uint64_t curr_time_left = 0u; p_motor_speed = (motor_speed_t *)pvParameters;
curr_time_left = time_us_64(); SemaphoreHandle_t *p_sem = p_motor_speed->sem;
float speed = 0.f;
float new_pwm = p_motor_speed->pwm_level;
uint64_t curr_time = 0u;
uint64_t prev_time = 0u;
uint64_t elapsed_time = 0u;
float control_signal = 0.f;
float integral = 0.f;
float prev_error = 0.f;
for (;;) for (;;)
{ {
if (xSemaphoreTake(g_wheel_speed_sem_left, pdMS_TO_TICKS(1000)) == if (xSemaphoreTake(*p_sem, pdMS_TO_TICKS(1000)) ==
pdTRUE) pdTRUE)
{ {
curr_time_left = time_us_64(); curr_time = time_us_64();
static uint64_t prev_time_left = 0u; elapsed_time = curr_time - prev_time;
static uint64_t elapsed_time_left = 1u; // to avoid division by 0 prev_time = curr_time;
elapsed_time_left = curr_time_left - prev_time_left;
prev_time_left = curr_time_left;
// speed in cm/s; speed = distance / time // speed in cm/s; speed = distance / time
// distance = circumference / 20 // distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm // distance = 20.4203522483 cm / 20 = 1.02101761242 cm
speed_left speed = (float)(1.02101761242f / (elapsed_time / 1000000.f));
= (float)(1.02101761242f / (elapsed_time_left / 1000000.f));
printf("left speed: %f cm/s\n", speed_left); printf("speed: %f cm/s\n", speed);
} }
else else
{ {
printf("left speed: 0 cm/s\n"); printf("speed: 0 cm/s\n");
} }
static float control_signal = 0.f; control_signal = compute_pid(&(p_motor_speed->target_speed),
static float integral = 0.f; &speed,
static float prev_error = 0.f; &integral,
&prev_error);
control_signal = compute_pid(
*(float *)pvParameters, speed_left, &integral, &prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED) if (new_pwm + control_signal > MAX_SPEED)
{ {
@ -139,66 +130,8 @@ monitor_left_wheel_speed_task(void *pvParameters)
printf("control signal: %f\n", control_signal); printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm); printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 0u); pwm_set_chan_level(*p_motor_speed->p_slice_num,
} p_motor_speed->channel,
} (int16_t) new_pwm);
void
monitor_right_wheel_speed_task(void *pvParameters)
{
static volatile float speed_right = 0.f;
static volatile uint64_t curr_time_right = 0u;
curr_time_right = time_us_64();
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_right, pdMS_TO_TICKS(1000)) ==
pdTRUE)
{
curr_time_right = time_us_64();
static uint64_t prev_time_right = 0u;
static uint64_t elapsed_time_right = 1u; // to avoid division by 0
elapsed_time_right = curr_time_right - prev_time_right;
prev_time_right = curr_time_right;
speed_right
= (float)(1.02101761242f / (elapsed_time_right / 1000000.f));
printf("right speed: %f cm/s\n", speed_right);
}
else
{
curr_time_right = time_us_64();
printf("right speed: 0 cm/s\n");
}
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(
*(float *)pvParameters, speed_right, &integral, &prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 1u);
} }
} }

32
frtos/motor/motor_test.c
View File

@ -19,23 +19,39 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true); irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL; static volatile motor_speed_t * p_motor_speed_left = NULL;
static volatile float target_speed = 30.0f; // cm/s static volatile motor_speed_t motor_speed_left = {
p_target_speed = &target_speed; .target_speed = 40.0f,
.pwm_level = 0u,
.sem = &g_wheel_speed_sem_left,
.p_slice_num = &g_slice_num_left,
.channel = PWM_CHAN_A
};
p_motor_speed_left = &motor_speed_left;
static volatile motor_speed_t * p_motor_speed_right = NULL;
static volatile motor_speed_t motor_speed_right = {
.target_speed = 20.0f,
.pwm_level = 0u,
.sem = &g_wheel_speed_sem_right,
.p_slice_num = &g_slice_num_right,
.channel = PWM_CHAN_B
};
p_motor_speed_right = &motor_speed_right;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_left_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task", "monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *) p_target_speed, (void *) p_motor_speed_left,
READ_LEFT_WHEEL_SPEED_PRIO, READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_right_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_right_wheel_speed_task", "monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *) p_target_speed, (void *)p_motor_speed_right,
READ_RIGHT_WHEEL_SPEED_PRIO, READ_RIGHT_WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);

34
frtos/rtos_car.c
View File

@ -21,36 +21,6 @@
void void
launch() launch()
{ {
// 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,
h_wheel_sensor_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, h_wheel_sensor_isr_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL;
static volatile float target_speed = 20.0f; // cm/s
p_target_speed = &target_speed;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_left_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_right_wheel_speed_task,
"monitor_right_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_RIGHT_WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
vTaskStartScheduler(); vTaskStartScheduler();
} }
@ -60,10 +30,6 @@ main (void)
{ {
stdio_usb_init(); stdio_usb_init();
motor_init();
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
launch(); launch();
return (0); return (0);