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 names

This commit is contained in:
Richie 2023-10-27 13:11:41 +08:00
parent a40fa548c3
commit 8cd0cd9571
5 changed files with 49 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
frtos/config/motor_config.h
View File

@ -34,8 +34,8 @@
* @param distance_cm Distance travelled in cm
*/
typedef struct {
float target_speed_cms;
float current_speed_cms;
float target_cms;
float current_cms;
float distance_cm;
} motor_speed_t;
@ -47,8 +47,8 @@ typedef struct {
*/
typedef struct {
uint slice_num;
uint pwm_channel;
uint16_t pwm_level;
uint channel;
uint16_t level;
} motor_pwm_t;
/*!
@ -58,9 +58,9 @@ typedef struct {
* @param pid_kd Derivative gain
*/
typedef struct {
float pid_kp;
float pid_ki;
float pid_kd;
float kp_value;
float ki_value;
float kd_value;
} motor_pid_t;
/*!

20
frtos/motor/motor_init.h
View File

@ -27,19 +27,19 @@
* 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,
motor_t g_motor_left = { .pwm.level = 0u,
.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,};
.pid.kp_value = 8.4f,
.pid.ki_value = 0.021f,
.pid.kd_value = 42.f,};
motor_t g_motor_right = { .pwm.pwm_level = 0u,
.pwm.pwm_channel = PWM_CHAN_B,
motor_t g_motor_right = { .pwm.level = 0u,
.pwm.channel = PWM_CHAN_B,
.speed.distance_cm = 0.0f,
.pid.pid_kp = 0.0f,
.pid.pid_ki = 0.0f,
.pid.pid_kd = 0.0f,};
.pid.kp_value = 0.0f,
.pid.ki_value = 0.0f,
.pid.kd_value = 0.0f,};
void
motor_init(void)

32
frtos/motor/motor_pid.h
View File

@ -20,16 +20,16 @@ compute_pid(const volatile motor_t *p_motor,
float *integral,
float *prev_error)
{
float error = p_motor->speed.target_speed_cms - p_motor->speed.current_speed_cms;
float error = p_motor->speed.target_cms - p_motor->speed.current_cms;
*integral += error;
float derivative = error - *prev_error;
float control_signal
= p_motor->pid.pid_kp * error +
p_motor->pid.pid_ki * (*integral) +
p_motor->pid.pid_kd * derivative;
= p_motor->pid.kp_value * error +
p_motor->pid.ki_value * (*integral) +
p_motor->pid.kd_value * derivative;
*prev_error = error;
@ -45,37 +45,37 @@ motor_pid_task(void *p_param)
for (;;)
{
if (p_motor->speed.target_speed_cms == 0.0f)
if (p_motor->speed.target_cms == 0.0f)
{
p_motor->pwm.pwm_level = 0;
p_motor->pwm.level = 0;
pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.pwm_channel,
p_motor->pwm.pwm_level);
p_motor->pwm.channel,
p_motor->pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
continue;
}
float control_signal = compute_pid(p_motor, &integral, &prev_error);
if (p_motor->pwm.pwm_level + control_signal > MAX_SPEED)
if (p_motor->pwm.level + control_signal > MAX_SPEED)
{
p_motor->pwm.pwm_level = MAX_SPEED;
p_motor->pwm.level = MAX_SPEED;
}
else if (p_motor->pwm.pwm_level + control_signal < MIN_SPEED)
else if (p_motor->pwm.level + control_signal < MIN_SPEED)
{
p_motor->pwm.pwm_level = MIN_SPEED;
p_motor->pwm.level = MIN_SPEED;
}
else
{
p_motor->pwm.pwm_level = p_motor->pwm.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->pwm.slice_num,
p_motor->pwm.pwm_channel,
p_motor->pwm.pwm_level);
p_motor->pwm.channel,
p_motor->pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
}

9
frtos/motor/motor_speed.h
View File

@ -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_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");
}

24
frtos/motor/motor_test.c
View File

@ -11,30 +11,30 @@ test_speed_change_task(void *p_param)
{
for (;;)
{
g_motor_left.speed.target_speed_cms = 30.0f;
g_motor_right.speed.target_speed_cms = 30.0f;
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_speed_cms = 20.0f;
g_motor_right.speed.target_speed_cms = 20.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_speed_cms = 0.0f;
g_motor_right.speed.target_speed_cms = 0.0f;
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_speed_cms = 30.0f;
g_motor_right.speed.target_speed_cms = 30.0f;
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_speed_cms = 20.0f;
g_motor_right.speed.target_speed_cms = 20.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_speed_cms = 0.0f;
g_motor_right.speed.target_speed_cms = 0.0f;
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);