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

20
frtos/motor/motor_init.h
View File

@ -27,19 +27,19 @@
* Ki = Kp / Tu = 0.021 * Ki = Kp / Tu = 0.021
* Kd = Kp * Tu / 8 = 42 * Kd = Kp * Tu / 8 = 42
*/ */
motor_t g_motor_left = { .pwm.pwm_level = 0u, motor_t g_motor_left = { .pwm.level = 0u,
.pwm.pwm_channel = PWM_CHAN_A, .pwm.channel = PWM_CHAN_A,
.speed.distance_cm = 0.0f, .speed.distance_cm = 0.0f,
.pid.pid_kp = 8.4f, .pid.kp_value = 8.4f,
.pid.pid_ki = 0.021f, .pid.ki_value = 0.021f,
.pid.pid_kd = 42.f,}; .pid.kd_value = 42.f,};
motor_t g_motor_right = { .pwm.pwm_level = 0u, motor_t g_motor_right = { .pwm.level = 0u,
.pwm.pwm_channel = PWM_CHAN_B, .pwm.channel = PWM_CHAN_B,
.speed.distance_cm = 0.0f, .speed.distance_cm = 0.0f,
.pid.pid_kp = 0.0f, .pid.kp_value = 0.0f,
.pid.pid_ki = 0.0f, .pid.ki_value = 0.0f,
.pid.pid_kd = 0.0f,}; .pid.kd_value = 0.0f,};
void void
motor_init(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 *integral,
float *prev_error) 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; *integral += error;
float derivative = error - *prev_error; float derivative = error - *prev_error;
float control_signal float control_signal
= p_motor->pid.pid_kp * error + = p_motor->pid.kp_value * error +
p_motor->pid.pid_ki * (*integral) + p_motor->pid.ki_value * (*integral) +
p_motor->pid.pid_kd * derivative; p_motor->pid.kd_value * derivative;
*prev_error = error; *prev_error = error;
@ -45,37 +45,37 @@ motor_pid_task(void *p_param)
for (;;) 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, pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.pwm_channel, p_motor->pwm.channel,
p_motor->pwm.pwm_level); p_motor->pwm.level);
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
continue; continue;
} }
float control_signal = compute_pid(p_motor, &integral, &prev_error); 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 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("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, pwm_set_chan_level(p_motor->pwm.slice_num,
p_motor->pwm.pwm_channel, p_motor->pwm.channel,
p_motor->pwm.pwm_level); p_motor->pwm.level);
vTaskDelay(pdMS_TO_TICKS(50)); 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 // 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_cms
(elapsed_time / = (float) (1.02101761242f / (elapsed_time / 1000000.f));
1000000.f));
p_motor->speed.distance_cm += 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 else
{ {
p_motor->speed.current_speed_cms = 0.f; p_motor->speed.current_cms = 0.f;
// printf("stopped\n"); // printf("stopped\n");
} }

24
frtos/motor/motor_test.c
View File

@ -11,30 +11,30 @@ test_speed_change_task(void *p_param)
{ {
for (;;) for (;;)
{ {
g_motor_left.speed.target_speed_cms = 30.0f; g_motor_left.speed.target_cms = 30.0f;
g_motor_right.speed.target_speed_cms = 30.0f; g_motor_right.speed.target_cms = 30.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 20.0f; g_motor_left.speed.target_cms = 20.0f;
g_motor_right.speed.target_speed_cms = 20.0f; g_motor_right.speed.target_cms = 20.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 0.0f; g_motor_left.speed.target_cms = 0.0f;
g_motor_right.speed.target_speed_cms = 0.0f; g_motor_right.speed.target_cms = 0.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
set_wheel_direction(DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD); set_wheel_direction(DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD);
g_motor_left.speed.target_speed_cms = 30.0f; g_motor_left.speed.target_cms = 30.0f;
g_motor_right.speed.target_speed_cms = 30.0f; g_motor_right.speed.target_cms = 30.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 20.0f; g_motor_left.speed.target_cms = 20.0f;
g_motor_right.speed.target_speed_cms = 20.0f; g_motor_right.speed.target_cms = 20.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
g_motor_left.speed.target_speed_cms = 0.0f; g_motor_left.speed.target_cms = 0.0f;
g_motor_right.speed.target_speed_cms = 0.0f; g_motor_right.speed.target_cms = 0.0f;
vTaskDelay(pdMS_TO_TICKS(5000)); vTaskDelay(pdMS_TO_TICKS(5000));
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);