From e8f8d31b8565d0d600a74d3cb6bfece30996bc32 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Sun, 29 Oct 2023 17:54:33 +0800 Subject: [PATCH] pending PID tuning; now car moves in S shape --- frtos/config/motor_config.h | 1 - frtos/motor/motor_direction.h | 26 ++++++++++++--- frtos/motor/motor_init.h | 14 ++------ frtos/motor/motor_pid.h | 63 +++++++++++++++++------------------ frtos/motor/motor_speed.h | 7 +--- frtos/motor/motor_test.c | 55 ++---------------------------- 6 files changed, 58 insertions(+), 108 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 86b995d..267d672 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -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; diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 1fe604c..f81273f 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -17,14 +17,30 @@ * @param right_speed The speed of the right motor, from 0.0 to 1.0 */ void -set_wheel_direction (uint32_t direction) +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); +} \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index da9b684..5a4f2ed 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -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) diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 2eedd86..af112da 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -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; + 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)); } diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index f53dcdd..520fbee 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -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); } } \ No newline at end of file diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index c248610..4da2f21 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -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(); }