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

update struct and pid for both wheels

This commit is contained in:
Richie 2023-10-27 13:07:55 +08:00
parent 1e34a8259f
commit a40fa548c3
5 changed files with 152 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
frtos/config/motor_config.h
View File

@ -24,36 +24,57 @@
#define PWM_CLK_DIV 250.f #define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U #define PWM_WRAP 5000U
/*
* ultimate gain Ku about 14, ultimate period Tu about 8 * 50 = 400ms
* Ku = 14, Tu = 400ms,
* Kp = 0.6 * Ku = 8.4
* Ki = Kp / Tu = 0.021
* Kd = Kp * Tu / 8 = 42
*/
#define PID_KP 8.4f
#define PID_KI 0.021f // 0.005f
#define PID_KD 42.f // 0.05f
#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 * @brief Structure for the motor speed parameters
* @param target_speed The target speed of the wheel, in cm/s * @param target_speed_cms Target speed in cm/s
* @param pwm_level The pwm level of the wheel, from 0 to 5000 * @param current_speed_cms Current speed in cm/s
* @param sem The semaphore for the wheel * @param distance_cm Distance travelled in cm
* @param p_slice_num The pointer to the slice number of the wheel
* @param channel The pwm channel of the wheel, left A or right B
*/ */
typedef struct { typedef struct {
float target_speed_cms; float target_speed_cms;
float current_speed_cms; float current_speed_cms;
uint16_t pwm_level; float distance_cm;
SemaphoreHandle_t sem;
uint slice_num;
uint pwm_channel;
float distance;
} motor_speed_t; } motor_speed_t;
/*!
* @brief Structure for the motor PWM parameters
* @param slice_num PWM slice number
* @param pwm_channel PWM channel, either A or B
* @param pwm_level PWM level, from 0 to 5000
*/
typedef struct {
uint slice_num;
uint pwm_channel;
uint16_t pwm_level;
} motor_pwm_t;
/*!
* @brief Structure for the motor PID parameters
* @param pid_kp Proportional gain
* @param pid_ki Integral gain
* @param pid_kd Derivative gain
*/
typedef struct {
float pid_kp;
float pid_ki;
float pid_kd;
} motor_pid_t;
/*!
* @brief Structure for the motor parameters
* @param speed Motor speed parameters
* @param sem Semaphore for the motor speed
* @param pwm Motor PWM parameters
* @param pid Motor PID parameters
*/
typedef struct {
motor_speed_t speed;
SemaphoreHandle_t sem;
motor_pwm_t pwm;
motor_pid_t pid;
} motor_t;
#endif /* MOTOR_CONFIG_H */ #endif /* MOTOR_CONFIG_H */

46
frtos/motor/motor_init.h
View File

@ -19,20 +19,34 @@
#include "motor_config.h" #include "motor_config.h"
motor_speed_t g_motor_speed_left = { .pwm_level = 2500u, // TODO: tune pid for both wheels again
.pwm_channel = PWM_CHAN_A, /*
.distance = 0.0f,}; * ultimate gain Ku about 14, ultimate period Tu about 8 * 50 = 400ms
* Ku = 14, Tu = 400ms,
* Kp = 0.6 * Ku = 8.4
* Ki = Kp / Tu = 0.021
* Kd = Kp * Tu / 8 = 42
*/
motor_t g_motor_left = { .pwm.pwm_level = 0u,
.pwm.pwm_channel = PWM_CHAN_A,
.speed.distance_cm = 0.0f,
.pid.pid_kp = 8.4f,
.pid.pid_ki = 0.021f,
.pid.pid_kd = 42.f,};
motor_speed_t g_motor_speed_right = { .pwm_level = 2500u, motor_t g_motor_right = { .pwm.pwm_level = 0u,
.pwm_channel = PWM_CHAN_B, .pwm.pwm_channel = PWM_CHAN_B,
.distance = 0.0f,}; .speed.distance_cm = 0.0f,
.pid.pid_kp = 0.0f,
.pid.pid_ki = 0.0f,
.pid.pid_kd = 0.0f,};
void void
motor_init(void) motor_init(void)
{ {
// Semaphore // Semaphore
g_motor_speed_left.sem = xSemaphoreCreateBinary(); g_motor_left.sem = xSemaphoreCreateBinary();
g_motor_speed_right.sem = xSemaphoreCreateBinary(); g_motor_right.sem = xSemaphoreCreateBinary();
gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_RIGHT);
gpio_init(SPEED_PIN_LEFT); gpio_init(SPEED_PIN_LEFT);
@ -54,21 +68,21 @@ motor_init(void)
gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
g_motor_speed_left.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT); g_motor_left.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
g_motor_speed_right.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); g_motor_right.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default // NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 250 = 500kHz // 125MHz / 250 = 500kHz
pwm_set_clkdiv(g_motor_speed_left.slice_num, PWM_CLK_DIV); pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_speed_right.slice_num, PWM_CLK_DIV); pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV);
// have them to be 500kHz / 5000 = 100Hz // have them to be 500kHz / 5000 = 100Hz
pwm_set_wrap(g_motor_speed_left.slice_num, (PWM_WRAP - 1U)); pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(g_motor_speed_right.slice_num, (PWM_WRAP - 1U)); pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U));
pwm_set_enabled(g_motor_speed_left.slice_num, true); pwm_set_enabled(g_motor_left.pwm.slice_num, true);
pwm_set_enabled(g_motor_speed_right.slice_num, true); pwm_set_enabled(g_motor_right.pwm.slice_num, true);
} }
#endif /* MOTOR_INIT_H */ #endif /* MOTOR_INIT_H */

48
frtos/motor/motor_pid.h
View File

@ -16,18 +16,20 @@
* @return The control signal * @return The control signal
*/ */
float float
compute_pid(const volatile float *target_speed, compute_pid(const volatile motor_t *p_motor,
const volatile float *current_speed,
float *integral, float *integral,
float *prev_error) float *prev_error)
{ {
float error = *target_speed - *current_speed; float error = p_motor->speed.target_speed_cms - p_motor->speed.current_speed_cms;
*integral += error; *integral += error;
float derivative = error - *prev_error; float derivative = error - *prev_error;
float control_signal float control_signal
= PID_KP * error + PID_KI * (*integral) + PID_KD * derivative; = p_motor->pid.pid_kp * error +
p_motor->pid.pid_ki * (*integral) +
p_motor->pid.pid_kd * derivative;
*prev_error = error; *prev_error = error;
@ -37,35 +39,43 @@ compute_pid(const volatile float *target_speed,
void void
motor_pid_task(void *p_param) motor_pid_task(void *p_param)
{ {
motor_speed_t *p_motor_speed = p_param; motor_t *p_motor = p_param;
float integral = 0.0f; float integral = 0.0f;
float prev_error = 0.0f; float prev_error = 0.0f;
for (;;) for (;;)
{ {
float control_signal = compute_pid(&(p_motor_speed->target_speed_cms), if (p_motor->speed.target_speed_cms == 0.0f)
&(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; p_motor->pwm.pwm_level = 0;
pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.pwm_channel,
p_motor->pwm.pwm_level);
vTaskDelay(pdMS_TO_TICKS(50));
continue;
} }
else if (p_motor_speed->pwm_level + control_signal < MIN_SPEED)
float control_signal = compute_pid(p_motor, &integral, &prev_error);
if (p_motor->pwm.pwm_level + control_signal > MAX_SPEED)
{ {
p_motor_speed->pwm_level = MIN_SPEED; p_motor->pwm.pwm_level = MAX_SPEED;
}
else if (p_motor->pwm.pwm_level + control_signal < MIN_SPEED)
{
p_motor->pwm.pwm_level = MIN_SPEED;
} }
else else
{ {
p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal; p_motor->pwm.pwm_level = p_motor->pwm.pwm_level + control_signal;
} }
// printf("control signal: %f\n", control_signal); // printf("control signal: %f\n", control_signal);
// printf("new pwm: %hu\n\n", p_motor_speed->pwm_level); // printf("new pwm: %hu\n\n", p_motor_speed->pwm_level);
pwm_set_chan_level(p_motor_speed->slice_num, pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor_speed->pwm_channel, p_motor->pwm.pwm_channel,
p_motor_speed->pwm_level); p_motor->pwm.pwm_level);
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
} }

16
frtos/motor/motor_speed.h
View File

@ -17,7 +17,7 @@ h_wheel_sensor_isr_handler(void)
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_motor_speed_left.sem, xSemaphoreGiveFromISR(g_motor_left.sem,
&xHigherPriorityTaskWoken); &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
@ -27,7 +27,7 @@ h_wheel_sensor_isr_handler(void)
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_motor_speed_right.sem, xSemaphoreGiveFromISR(g_motor_right.sem,
&xHigherPriorityTaskWoken); &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
@ -42,8 +42,8 @@ h_wheel_sensor_isr_handler(void)
void void
monitor_wheel_speed_task(void *pvParameters) monitor_wheel_speed_task(void *pvParameters)
{ {
volatile motor_speed_t *p_motor_speed = NULL; volatile motor_t *p_motor = NULL;
p_motor_speed = (motor_speed_t *)pvParameters; p_motor = (motor_t *)pvParameters;
uint64_t curr_time = 0u; uint64_t curr_time = 0u;
uint64_t prev_time = 0u; uint64_t prev_time = 0u;
@ -51,7 +51,7 @@ monitor_wheel_speed_task(void *pvParameters)
for (;;) for (;;)
{ {
if (xSemaphoreTake(p_motor_speed->sem, pdMS_TO_TICKS(100)) if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100))
== pdTRUE) == pdTRUE)
{ {
curr_time = time_us_64(); curr_time = time_us_64();
@ -62,17 +62,17 @@ monitor_wheel_speed_task(void *pvParameters)
// 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
p_motor_speed->current_speed_cms = (float) (1.02101761242f / p_motor->speed.current_speed_cms = (float) (1.02101761242f /
(elapsed_time / (elapsed_time /
1000000.f)); 1000000.f));
p_motor_speed->distance += 1.02101761242f; p_motor->speed.distance_cm += 1.02101761242f;
// printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms); // printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms);
} }
else else
{ {
p_motor_speed->current_speed_cms = 0.f; p_motor->speed.current_speed_cms = 0.f;
// printf("stopped\n"); // printf("stopped\n");
} }

64
frtos/motor/motor_test.c
View File

@ -11,18 +11,34 @@ test_speed_change_task(void *p_param)
{ {
for (;;) for (;;)
{ {
g_motor_speed_left.target_speed_cms = 15.0f; g_motor_left.speed.target_speed_cms = 30.0f;
g_motor_speed_right.target_speed_cms = 15.0f; g_motor_right.speed.target_speed_cms = 30.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
// g_motor_speed_left.target_speed_cms = 20.0f; g_motor_left.speed.target_speed_cms = 20.0f;
// g_motor_speed_right.target_speed_cms = 20.0f; g_motor_right.speed.target_speed_cms = 20.0f;
// 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)); vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 0.0f;
g_motor_right.speed.target_speed_cms = 0.0f;
vTaskDelay(pdMS_TO_TICKS(5000));
set_wheel_direction(DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD);
g_motor_left.speed.target_speed_cms = 30.0f;
g_motor_right.speed.target_speed_cms = 30.0f;
vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 20.0f;
g_motor_right.speed.target_speed_cms = 20.0f;
vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 0.0f;
g_motor_right.speed.target_speed_cms = 0.0f;
vTaskDelay(pdMS_TO_TICKS(5000));
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
} }
} }
@ -41,21 +57,21 @@ launch()
// Left wheel // Left wheel
// //
// TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
// xTaskCreate(monitor_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 *)&g_motor_speed_left, (void *)&g_motor_left,
// WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
// &h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
// TaskHandle_t h_motor_pid_left_task_handle = NULL; TaskHandle_t h_motor_pid_left_task_handle = NULL;
// xTaskCreate(motor_pid_task, xTaskCreate(motor_pid_task,
// "motor_pid_task", "motor_pid_task",
// configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
// (void *)&g_motor_speed_left, (void *)&g_motor_left,
// WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
// &h_motor_pid_left_task_handle); &h_motor_pid_left_task_handle);
// Right wheel // Right wheel
// //
@ -63,7 +79,7 @@ launch()
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task", "monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)&g_motor_speed_right, (void *)&g_motor_right,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
@ -71,7 +87,7 @@ launch()
xTaskCreate(motor_pid_task, xTaskCreate(motor_pid_task,
"motor_pid_task", "motor_pid_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)&g_motor_speed_right, (void *)&g_motor_right,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_motor_pid_right_task_handle); &h_motor_pid_right_task_handle);