From c19b665e0bb610411103be0a61bc5c142208e39e Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Mon, 6 Nov 2023 23:46:47 +0800 Subject: [PATCH] update PWM and PID value --- frtos/config/motor_config.h | 10 ++++++---- frtos/motor/motor_init.h | 16 +++++++++++----- frtos/motor/motor_pid.h | 16 ++++++++-------- frtos/motor/motor_speed.h | 7 ++++++- frtos/motor/motor_test.c | 16 ++++++++++++++++ 5 files changed, 47 insertions(+), 18 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 509f9f9..38a81ea 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -28,11 +28,11 @@ #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U -#define PWM_CLK_DIV 250.f -#define PWM_WRAP 5000U +#define PWM_CLK_DIV 50.f +#define PWM_WRAP 100U -#define MAX_SPEED 4900U -#define MIN_SPEED 0U // To be changed +#define MAX_PWM_LEVEL 99U +#define MIN_PWM_LEVEL 0U #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) #define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) @@ -97,4 +97,6 @@ typedef struct volatile bool is_running; } distance_to_stop_t; +volatile bool g_use_pid = true; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 56cb390..7ac9b70 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -23,12 +23,17 @@ motor_t g_motor_left = { .pwm.level = 0u, .pwm.channel = PWM_CHAN_A, .speed.distance_cm = 0.0f }; +// classic ziegler nichols method +// Ku = 1000, Tu = 0.9s, interval = 0.05s +// Kp = 0.6 * Ku = 600 +// Ki = 2 * Kp * 0.05 / Tu = 66.67 +// Kd = Kp * Tu * 0.05 / 8 = 1350 motor_t g_motor_right = { .pwm.level = 0u, .pwm.channel = PWM_CHAN_B, .speed.distance_cm = 0.0f, - .pid.kp_value = 1000.f, - .pid.ki_value = 0.0f, - .pid.kd_value = 10000.0f,}; + .pid.kp_value = 600.f, + .pid.ki_value = 66.67f, + .pid.kd_value = 1350.f,}; void motor_init(void) @@ -62,11 +67,12 @@ motor_init(void) // NOTE: PWM clock is 125MHz for raspberrypi pico w by default - // 125MHz / 250 = 500kHz + // 125MHz / 50 = 2500kHz 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 + // L289N can accept up to 40kHz + // 2500kHz / 100 = 25kHz pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 16283f1..1930ed6 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -21,7 +21,7 @@ compute_pid(float *integral, float *prev_error) float error = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; - printf("%f,\n", error); + printf("%f\n", error); *integral += error; @@ -42,7 +42,7 @@ repeating_pid_handler(__unused struct repeating_timer *t) static float integral = 0.0f; static float prev_error = 0.0f; - if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u)) + if (!g_use_pid) { return true; } @@ -51,14 +51,14 @@ repeating_pid_handler(__unused struct repeating_timer *t) float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; - if (temp > MAX_SPEED) + if (temp > MAX_PWM_LEVEL) { - temp = MAX_SPEED; + temp = MAX_PWM_LEVEL; } - if (temp < MIN_SPEED) + if (temp <= MIN_PWM_LEVEL) { - temp = MIN_SPEED + 1u; + temp = MIN_PWM_LEVEL + 1u; } g_motor_right.pwm.level = (uint16_t)temp; @@ -66,8 +66,8 @@ repeating_pid_handler(__unused struct repeating_timer *t) g_motor_right.pwm.channel, g_motor_right.pwm.level); - printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); - printf("distance: %f cm\n", g_motor_right.speed.distance_cm); +// printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); +// printf("distance: %f cm\n", g_motor_right.speed.distance_cm); return true; } \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 4848f83..53891be 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -86,11 +86,16 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) /*! * @brief Set the speed of the wheels - * @param pwm_level The pwm_level of the wheels, from 0 to 5000 + * @param pwm_level The pwm_level of the wheels, from 0 to 99 */ void set_wheel_speed_synced(uint32_t pwm_level) { + if (pwm_level > MAX_PWM_LEVEL) + { + pwm_level = MAX_PWM_LEVEL; + } + set_wheel_speed(pwm_level, &g_motor_left); set_wheel_speed(pwm_level, &g_motor_right); } diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index b7dbe1f..8899ebb 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -4,6 +4,22 @@ #include "motor_direction.h" #include "motor_pid.h" +void +motor_control_task(__unused void *params) +{ + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90); + + vTaskDelay(pdMS_TO_TICKS(10000)); + + revert_wheel_direction(); + set_wheel_speed_synced(90); + + vTaskDelay(pdMS_TO_TICKS(10000)); + } +} void launch()