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

Merge remote-tracking branch 'origin/main'

This commit is contained in:
Devoalda 2023-10-27 19:54:30 +08:00
parent 9ae974deab 54b7cd1057
commit c3b7647422
5 changed files with 155 additions and 95 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_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 MIN_SPEED 0U // To be changed
/*!
* @brief Structure for the motor speed
* @param target_speed The target speed of the wheel, in cm/s
* @param pwm_level The pwm level of the wheel, from 0 to 5000
* @param sem The semaphore for the wheel
* @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
* @brief Structure for the motor speed parameters
* @param target_speed_cms Target speed in cm/s
* @param current_speed_cms Current speed in cm/s
* @param distance_cm Distance travelled in cm
*/
typedef struct {
float target_speed_cms;
float current_speed_cms;
uint16_t pwm_level;
SemaphoreHandle_t sem;
uint slice_num;
uint pwm_channel;
float distance;
float target_cms;
float current_cms;
float distance_cm;
} 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 channel;
uint16_t 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 kp_value;
float ki_value;
float kd_value;
} 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 */

46
frtos/motor/motor_init.h
View File

@ -19,20 +19,34 @@
#include "motor_config.h"
motor_speed_t g_motor_speed_left = { .pwm_level = 2500u,
.pwm_channel = PWM_CHAN_A,
.distance = 0.0f,};
// TODO: tune pid for both wheels again
/*
* 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.level = 0u,
.pwm.channel = PWM_CHAN_A,
.speed.distance_cm = 0.0f,
.pid.kp_value = 8.4f,
.pid.ki_value = 0.021f,
.pid.kd_value = 42.f,};
motor_speed_t g_motor_speed_right = { .pwm_level = 2500u,
.pwm_channel = PWM_CHAN_B,
.distance = 0.0f,};
motor_t g_motor_right = { .pwm.level = 0u,
.pwm.channel = PWM_CHAN_B,
.speed.distance_cm = 0.0f,
.pid.kp_value = 0.0f,
.pid.ki_value = 0.0f,
.pid.kd_value = 0.0f,};
void
motor_init(void)
{
// Semaphore
g_motor_speed_left.sem = xSemaphoreCreateBinary();
g_motor_speed_right.sem = xSemaphoreCreateBinary();
g_motor_left.sem = xSemaphoreCreateBinary();
g_motor_right.sem = xSemaphoreCreateBinary();
gpio_init(SPEED_PIN_RIGHT);
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_RIGHT, GPIO_FUNC_PWM);
g_motor_speed_left.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_left.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
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
// 125MHz / 250 = 500kHz
pwm_set_clkdiv(g_motor_speed_left.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_speed_right.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV);
// have them to be 500kHz / 5000 = 100Hz
pwm_set_wrap(g_motor_speed_left.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(g_motor_speed_right.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(g_motor_left.pwm.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_speed_right.slice_num, true);
pwm_set_enabled(g_motor_left.pwm.slice_num, true);
pwm_set_enabled(g_motor_right.pwm.slice_num, true);
}
#endif /* MOTOR_INIT_H */

46
frtos/motor/motor_pid.h
View File

@ -16,18 +16,20 @@
* @return The control signal
*/
float
compute_pid(const volatile float *target_speed,
const volatile float *current_speed,
compute_pid(const volatile motor_t *p_motor,
float *integral,
float *prev_error)
{
float error = *target_speed - *current_speed;
float error = p_motor->speed.target_cms - p_motor->speed.current_cms;
*integral += error;
float derivative = error - *prev_error;
float control_signal
= PID_KP * error + PID_KI * (*integral) + PID_KD * derivative;
= p_motor->pid.kp_value * error +
p_motor->pid.ki_value * (*integral) +
p_motor->pid.kd_value * derivative;
*prev_error = error;
@ -37,35 +39,43 @@ compute_pid(const volatile float *target_speed,
void
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 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)
if (p_motor->speed.target_cms == 0.0f)
{
p_motor_speed->pwm_level = MAX_SPEED;
p_motor->pwm.level = 0;
pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.channel,
p_motor->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.level + control_signal > MAX_SPEED)
{
p_motor_speed->pwm_level = MIN_SPEED;
p_motor->pwm.level = MAX_SPEED;
}
else if (p_motor->pwm.level + control_signal < MIN_SPEED)
{
p_motor->pwm.level = MIN_SPEED;
}
else
{
p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal;
p_motor->pwm.level = p_motor->pwm.level + 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->level);
pwm_set_chan_level(p_motor_speed->slice_num,
p_motor_speed->pwm_channel,
p_motor_speed->pwm_level);
pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.channel,
p_motor->pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
}

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

64
frtos/motor/motor_test.c
View File

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