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

pending PID tuning; now car moves in S shape

This commit is contained in:
Richie 2023-10-29 17:54:33 +08:00
parent 05b5872a6c
commit e8f8d31b85
6 changed files with 58 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
frtos/config/motor_config.h
View File

@ -34,7 +34,6 @@
* @param distance_cm Distance travelled in cm
*/
typedef struct {
float target_cms;
float current_cms;
float distance_cm;
} motor_speed_t;

24
frtos/motor/motor_direction.h
View File

@ -19,12 +19,28 @@
void
set_wheel_direction(uint32_t direction)
{
static const uint32_t mask = DIRECTION_LEFT_FORWARD |
DIRECTION_LEFT_BACKWARD |
DIRECTION_RIGHT_FORWARD |
DIRECTION_RIGHT_BACKWARD;
static const uint32_t mask
= DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD
| DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD;
gpio_put_masked(mask, 0U);
gpio_set_mask(direction);
}
/*!
* @brief Set the speed of the wheels
* @param speed The speed of the wheels, from 0 to 5000
*/
void
set_wheel_speed(uint32_t speed)
{
g_motor_right.pwm.level = speed;
g_motor_left.pwm.level = speed;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
pwm_set_chan_level(g_motor_left.pwm.slice_num,
g_motor_left.pwm.channel,
g_motor_left.pwm.level);
}

14
frtos/motor/motor_init.h
View File

@ -19,14 +19,6 @@
#include "motor_config.h"
// 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,
@ -37,9 +29,9 @@ motor_t g_motor_left = { .pwm.level = 0u,
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,};
.pid.kp_value = 8.4f,
.pid.ki_value = 0.021f,
.pid.kd_value = 42.0f,};
void
motor_init(void)

59
frtos/motor/motor_pid.h
View File

@ -16,20 +16,20 @@
* @return The control signal
*/
float
compute_pid(const volatile motor_t *p_motor,
float *integral,
float *prev_error)
compute_pid(float *integral, float *prev_error)
{
float error = p_motor->speed.target_cms - p_motor->speed.current_cms;
float error
= g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
printf("error: %f\n", error);
*integral += error;
float derivative = error - *prev_error;
float control_signal
= p_motor->pid.kp_value * error +
p_motor->pid.ki_value * (*integral) +
p_motor->pid.kd_value * derivative;
float control_signal = g_motor_right.pid.kp_value * error
+ g_motor_right.pid.ki_value * (*integral)
+ g_motor_right.pid.kd_value * derivative;
*prev_error = error;
@ -37,45 +37,42 @@ compute_pid(const volatile motor_t *p_motor,
}
void
motor_pid_task(void *p_param)
motor_pid_task(__unused void *p_param)
{
motor_t *p_motor = p_param;
float integral = 0.0f;
float prev_error = 0.0f;
for (;;)
{
if (p_motor->speed.target_cms == 0.0f)
if (g_motor_left.pwm.level == 0u)
{
p_motor->pwm.level = 0;
pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.channel,
p_motor->pwm.level);
g_motor_right.pwm.level = 0;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
continue;
}
float control_signal = compute_pid(p_motor, &integral, &prev_error);
float control_signal = compute_pid(&integral, &prev_error);
if (p_motor->pwm.level + control_signal > MAX_SPEED)
float temp = (float) g_motor_right.pwm.level + control_signal;
if (temp > MAX_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->pwm.level = p_motor->pwm.level + control_signal;
temp = MAX_SPEED;
}
// printf("control signal: %f\n", control_signal);
// printf("new pwm: %hu\n\n", p_motor_speed->level);
if (temp < MIN_SPEED)
{
temp = MIN_SPEED;
}
pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.channel,
p_motor->pwm.level);
g_motor_right.pwm.level = (uint16_t) temp;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
}

9
frtos/motor/motor_speed.h
View File

@ -63,18 +63,13 @@ monitor_wheel_speed_task(void *pvParameters)
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm
p_motor->speed.current_cms
= (float) (1.02101761242f / (elapsed_time / 1000000.f));
= (float) (1021017.61242f / elapsed_time);
p_motor->speed.distance_cm += 1.02101761242f;
// printf("speed: %f cm/s\n", p_motor_speed->current_cms);
}
else
{
p_motor->speed.current_cms = 0.f;
// printf("stopped\n");
}
// printf("distance: %f cm\n", p_motor_speed->distance);
}
}
}

55
frtos/motor/motor_test.c
View File

@ -6,42 +6,6 @@
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
void
test_speed_change_task(void *p_param)
{
for (;;)
{
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_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);
}
}
void
launch()
{
@ -55,6 +19,9 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true);
// Set wheel speed
set_wheel_speed(3000);
// Left wheel
//
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
@ -65,13 +32,6 @@ 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_left,
WHEEL_SPEED_PRIO,
&h_motor_pid_left_task_handle);
// Right wheel
//
@ -91,15 +51,6 @@ launch()
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",
configMINIMAL_STACK_SIZE,
NULL,
WHEEL_SPEED_PRIO,
&h_test_speed_change_task_handle);
vTaskStartScheduler();
}