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

separate speed control and monitor

This commit is contained in:
Richie 2023-10-21 17:14:49 +08:00
parent b8850d80fb
commit ef965e0f2f
5 changed files with 119 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
frtos/config/motor_config.h
View File

@ -25,8 +25,8 @@
#define PWM_WRAP 5000U
#define PID_KP 3.f
#define PID_KI 0.01f
#define PID_KD 0.05f
#define PID_KI 0.01 // 0.01f
#define PID_KD 0.05f // 0.05f
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
@ -41,6 +41,7 @@
*/
typedef struct {
float target_speed_cms;
float current_speed_cms;
uint16_t pwm_level;
SemaphoreHandle_t * p_sem;
uint * p_slice_num;

72
frtos/motor/motor_pid.h Normal file
View File

@ -0,0 +1,72 @@
/*
* @file motor_pid.h
* @brief control the speed of the wheels by setting the PWM level, using PID
* controller
* @author Richie
*/
#include "motor_init.h"
/*!
* @brief Compute the control signal using PID controller
* @param target_speed The target speed of the wheel
* @param current_speed The current speed of the wheel
* @param integral The integral term of the PID controller
* @param prev_error The previous error of the PID controller
* @return The control signal
*/
float
compute_pid(const volatile float *target_speed,
const volatile float *current_speed,
float *integral,
float *prev_error)
{
float error = *target_speed - *current_speed;
*integral += error;
float derivative = error - *prev_error;
float control_signal
= PID_KP * error + PID_KI * (*integral) + PID_KD * derivative;
*prev_error = error;
return control_signal;
}
void
motor_pid_task(void *p_param)
{
motor_speed_t *p_motor_speed = p_param;
float integral = 0.0f;
float prev_error = 0.0f;
for (;;)
{
float control_signal = compute_pid(&(p_motor_speed->target_speed_cms),
&(p_motor_speed->current_speed_cms),
&integral, &prev_error);
if (p_motor_speed->pwm_level + control_signal > MAX_SPEED)
{
p_motor_speed->pwm_level = MAX_SPEED;
}
else if (p_motor_speed->pwm_level + control_signal < MIN_SPEED)
{
p_motor_speed->pwm_level = MIN_SPEED;
}
else
{
p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %hu\n\n", p_motor_speed->pwm_level);
pwm_set_chan_level(*p_motor_speed->p_slice_num,
p_motor_speed->pwm_channel,
p_motor_speed->pwm_level);
vTaskDelay(pdMS_TO_TICKS(50));
}
}

70
frtos/motor/motor_speed.h
View File

@ -1,9 +1,6 @@
/*
* @file motor_speed.c
* @brief control the speed of the wheels by setting the PWM level, and
* monitor the speed by using edge interrupt and measure the time between
* each slot of the wheel, then calculate the speed of the wheel in cm/s, and
* adjust the speed of the wheel by using PID controller, and set the new PWM
* @brief monitor and update the speed of the wheels
* @author Richie
*/
@ -36,32 +33,6 @@ h_wheel_sensor_isr_handler(void)
}
}
/*!
* @brief Compute the control signal using PID controller
* @param target_speed The target speed of the wheel
* @param current_speed The current speed of the wheel
* @param integral The integral term of the PID controller
* @param prev_error The previous error of the PID controller
* @return The control signal
*/
float
compute_pid(const volatile float *target_speed,
const float *current_speed,
float *integral,
float *prev_error)
{
float error = *target_speed - *current_speed;
*integral += error;
float derivative = error - *prev_error;
float control_signal
= PID_KP * error + PID_KI * (*integral) + PID_KD * derivative;
*prev_error = error;
return control_signal;
}
/*!
* @brief Task to monitor and control the speed of the wheel
@ -74,18 +45,10 @@ monitor_wheel_speed_task(void *pvParameters)
volatile motor_speed_t *p_motor_speed = NULL;
p_motor_speed = (motor_speed_t *)pvParameters;
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 (;;)
{
if (xSemaphoreTake(*p_motor_speed->p_sem, pdMS_TO_TICKS(100))
@ -99,37 +62,16 @@ monitor_wheel_speed_task(void *pvParameters)
// distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm
speed = (float)(1.02101761242f / (elapsed_time / 1000000.f));
p_motor_speed->current_speed_cms = (float) (1.02101761242f /
(elapsed_time /
1000000.f));
printf("speed: %f cm/s\n", speed);
printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms);
}
else
{
speed = 0.f;
p_motor_speed->current_speed_cms = 0.f;
printf("stopped\n");
}
control_signal = compute_pid(
&(p_motor_speed->target_speed_cms), &speed, &integral, &prev_error);
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);
pwm_set_chan_level(*p_motor_speed->p_slice_num,
p_motor_speed->pwm_channel,
(int16_t)new_pwm);
}
}

36
frtos/motor/motor_test.c
View File

@ -2,6 +2,7 @@
#include "motor_speed.h"
#include "motor_direction.h"
#include "motor_pid.h"
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
@ -10,13 +11,18 @@ test_speed_change_task(void *p_param)
{
for (;;)
{
g_motor_speed_left.target_speed_cms = 50.0f;
g_motor_speed_right.target_speed_cms = 50.0f;
vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_speed_left.target_speed_cms = 20.0f;
g_motor_speed_right.target_speed_cms = 20.0f;
vTaskDelay(pdMS_TO_TICKS(10000));
vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_speed_left.target_speed_cms = 0.0f;
g_motor_speed_right.target_speed_cms = 0.0f;
vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_speed_left.target_speed_cms = 40.0f;
g_motor_speed_right.target_speed_cms = 40.0f;
vTaskDelay(pdMS_TO_TICKS(10000));
}
}
@ -33,6 +39,8 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true);
// Left wheel
//
// TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
// xTaskCreate(monitor_wheel_speed_task,
// "monitor_left_wheel_speed_task",
@ -41,6 +49,16 @@ launch()
// WHEEL_SPEED_PRIO,
// &h_monitor_left_wheel_speed_task_handle);
// TaskHandle_t h_motor_pid_left_task_handle = NULL;
// xTaskCreate(motor_pid_task,
// "motor_pid_task",
// configMINIMAL_STACK_SIZE,
// (void *)&g_motor_speed_left,
// WHEEL_SPEED_PRIO,
// &h_motor_pid_left_task_handle);
// Right wheel
//
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task",
@ -49,6 +67,16 @@ launch()
WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
TaskHandle_t h_motor_pid_right_task_handle = NULL;
xTaskCreate(motor_pid_task,
"motor_pid_task",
configMINIMAL_STACK_SIZE,
(void *)&g_motor_speed_right,
WHEEL_SPEED_PRIO,
&h_motor_pid_right_task_handle);
// Test speed change
//
TaskHandle_t h_test_speed_change_task_handle = NULL;
xTaskCreate(test_speed_change_task,
"test_speed_change_task",