From a40fa548c3bef9bc7dc9a45f69426b35ee264ba9 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 27 Oct 2023 13:07:55 +0800 Subject: [PATCH] update struct and pid for both wheels --- frtos/config/motor_config.h | 69 ++++++++++++++++++++++++------------- frtos/motor/motor_init.h | 46 ++++++++++++++++--------- frtos/motor/motor_pid.h | 48 ++++++++++++++++---------- frtos/motor/motor_speed.h | 16 ++++----- frtos/motor/motor_test.c | 64 +++++++++++++++++++++------------- 5 files changed, 152 insertions(+), 91 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 4219df3..1c8d231 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -24,36 +24,57 @@ #define PWM_CLK_DIV 250.f #define PWM_WRAP 5000U -/* - * 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 - */ -#define PID_KP 8.4f -#define PID_KI 0.021f // 0.005f -#define PID_KD 42.f // 0.05f - #define MAX_SPEED 4900U #define MIN_SPEED 0U // To be changed /*! - * @brief Structure for the motor speed - * @param target_speed The target speed of the wheel, in cm/s - * @param pwm_level The pwm level of the wheel, from 0 to 5000 - * @param sem The semaphore for the wheel - * @param p_slice_num The pointer to the slice number of the wheel - * @param channel The pwm channel of the wheel, left A or right B + * @brief Structure for the motor speed parameters + * @param target_speed_cms Target speed in cm/s + * @param current_speed_cms Current speed in cm/s + * @param distance_cm Distance travelled in cm */ typedef struct { - float target_speed_cms; - float current_speed_cms; - uint16_t pwm_level; - SemaphoreHandle_t sem; - uint slice_num; - uint pwm_channel; - float distance; + float target_speed_cms; + float current_speed_cms; + float distance_cm; } motor_speed_t; +/*! + * @brief Structure for the motor PWM parameters + * @param slice_num PWM slice number + * @param pwm_channel PWM channel, either A or B + * @param pwm_level PWM level, from 0 to 5000 + */ +typedef struct { + uint slice_num; + uint pwm_channel; + uint16_t pwm_level; +} motor_pwm_t; + +/*! + * @brief Structure for the motor PID parameters + * @param pid_kp Proportional gain + * @param pid_ki Integral gain + * @param pid_kd Derivative gain + */ +typedef struct { + float pid_kp; + float pid_ki; + float pid_kd; +} motor_pid_t; + +/*! + * @brief Structure for the motor parameters + * @param speed Motor speed parameters + * @param sem Semaphore for the motor speed + * @param pwm Motor PWM parameters + * @param pid Motor PID parameters + */ +typedef struct { + motor_speed_t speed; + SemaphoreHandle_t sem; + motor_pwm_t pwm; + motor_pid_t pid; +} motor_t; + #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 970b12e..576e08f 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,20 +19,34 @@ #include "motor_config.h" -motor_speed_t g_motor_speed_left = { .pwm_level = 2500u, - .pwm_channel = PWM_CHAN_A, - .distance = 0.0f,}; +// 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.pwm_level = 0u, + .pwm.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,}; -motor_speed_t g_motor_speed_right = { .pwm_level = 2500u, - .pwm_channel = PWM_CHAN_B, - .distance = 0.0f,}; +motor_t g_motor_right = { .pwm.pwm_level = 0u, + .pwm.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,}; void motor_init(void) { // Semaphore - g_motor_speed_left.sem = xSemaphoreCreateBinary(); - g_motor_speed_right.sem = xSemaphoreCreateBinary(); + g_motor_left.sem = xSemaphoreCreateBinary(); + g_motor_right.sem = xSemaphoreCreateBinary(); gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); @@ -54,21 +68,21 @@ motor_init(void) gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); - g_motor_speed_left.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT); - g_motor_speed_right.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); + g_motor_left.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT); + g_motor_right.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); // NOTE: PWM clock is 125MHz for raspberrypi pico w by default // 125MHz / 250 = 500kHz - pwm_set_clkdiv(g_motor_speed_left.slice_num, PWM_CLK_DIV); - pwm_set_clkdiv(g_motor_speed_right.slice_num, PWM_CLK_DIV); + 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 - pwm_set_wrap(g_motor_speed_left.slice_num, (PWM_WRAP - 1U)); - pwm_set_wrap(g_motor_speed_right.slice_num, (PWM_WRAP - 1U)); + pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); + pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_enabled(g_motor_speed_left.slice_num, true); - pwm_set_enabled(g_motor_speed_right.slice_num, true); + pwm_set_enabled(g_motor_left.pwm.slice_num, true); + pwm_set_enabled(g_motor_right.pwm.slice_num, true); } #endif /* MOTOR_INIT_H */ \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index cd4bf10..74e0b06 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -16,18 +16,20 @@ * @return The control signal */ float -compute_pid(const volatile float *target_speed, - const volatile float *current_speed, +compute_pid(const volatile motor_t *p_motor, float *integral, float *prev_error) { - float error = *target_speed - *current_speed; + float error = p_motor->speed.target_speed_cms - p_motor->speed.current_speed_cms; + *integral += error; float derivative = error - *prev_error; float control_signal - = PID_KP * error + PID_KI * (*integral) + PID_KD * derivative; + = p_motor->pid.pid_kp * error + + p_motor->pid.pid_ki * (*integral) + + p_motor->pid.pid_kd * derivative; *prev_error = error; @@ -37,35 +39,43 @@ compute_pid(const volatile float *target_speed, void motor_pid_task(void *p_param) { - motor_speed_t *p_motor_speed = p_param; - float integral = 0.0f; - float prev_error = 0.0f; + motor_t *p_motor = p_param; + float integral = 0.0f; + float prev_error = 0.0f; for (;;) { - float control_signal = compute_pid(&(p_motor_speed->target_speed_cms), - &(p_motor_speed->current_speed_cms), - &integral, &prev_error); - - if (p_motor_speed->pwm_level + control_signal > MAX_SPEED) + if (p_motor->speed.target_speed_cms == 0.0f) { - p_motor_speed->pwm_level = MAX_SPEED; + p_motor->pwm.pwm_level = 0; + pwm_set_chan_level(p_motor->pwm.slice_num, + p_motor->pwm.pwm_channel, + p_motor->pwm.pwm_level); + vTaskDelay(pdMS_TO_TICKS(50)); + continue; } - else if (p_motor_speed->pwm_level + control_signal < MIN_SPEED) + + float control_signal = compute_pid(p_motor, &integral, &prev_error); + + if (p_motor->pwm.pwm_level + control_signal > MAX_SPEED) { - p_motor_speed->pwm_level = MIN_SPEED; + p_motor->pwm.pwm_level = MAX_SPEED; + } + else if (p_motor->pwm.pwm_level + control_signal < MIN_SPEED) + { + p_motor->pwm.pwm_level = MIN_SPEED; } else { - p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal; + p_motor->pwm.pwm_level = p_motor->pwm.pwm_level + control_signal; } // printf("control signal: %f\n", control_signal); // printf("new pwm: %hu\n\n", p_motor_speed->pwm_level); - pwm_set_chan_level(p_motor_speed->slice_num, - p_motor_speed->pwm_channel, - p_motor_speed->pwm_level); + pwm_set_chan_level(p_motor->pwm.slice_num, + p_motor->pwm.pwm_channel, + p_motor->pwm.pwm_level); vTaskDelay(pdMS_TO_TICKS(50)); } diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 525601f..81b89d1 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -17,7 +17,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_motor_speed_left.sem, + xSemaphoreGiveFromISR(g_motor_left.sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -27,7 +27,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_motor_speed_right.sem, + xSemaphoreGiveFromISR(g_motor_right.sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -42,8 +42,8 @@ h_wheel_sensor_isr_handler(void) void monitor_wheel_speed_task(void *pvParameters) { - volatile motor_speed_t *p_motor_speed = NULL; - p_motor_speed = (motor_speed_t *)pvParameters; + volatile motor_t *p_motor = NULL; + p_motor = (motor_t *)pvParameters; uint64_t curr_time = 0u; uint64_t prev_time = 0u; @@ -51,7 +51,7 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if (xSemaphoreTake(p_motor_speed->sem, pdMS_TO_TICKS(100)) + if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) == pdTRUE) { curr_time = time_us_64(); @@ -62,17 +62,17 @@ 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 / + p_motor->speed.current_speed_cms = (float) (1.02101761242f / (elapsed_time / 1000000.f)); - p_motor_speed->distance += 1.02101761242f; + p_motor->speed.distance_cm += 1.02101761242f; // printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms); } else { - p_motor_speed->current_speed_cms = 0.f; + p_motor->speed.current_speed_cms = 0.f; // printf("stopped\n"); } diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 0e9268b..5644eab 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -11,18 +11,34 @@ test_speed_change_task(void *p_param) { for (;;) { - g_motor_speed_left.target_speed_cms = 15.0f; - g_motor_speed_right.target_speed_cms = 15.0f; + g_motor_left.speed.target_speed_cms = 30.0f; + g_motor_right.speed.target_speed_cms = 30.0f; vTaskDelay(pdMS_TO_TICKS(5000)); -// g_motor_speed_left.target_speed_cms = 20.0f; -// g_motor_speed_right.target_speed_cms = 20.0f; -// vTaskDelay(pdMS_TO_TICKS(5000)); - - g_motor_speed_left.target_speed_cms = 0.0f; - g_motor_speed_right.target_speed_cms = 0.0f; + g_motor_left.speed.target_speed_cms = 20.0f; + g_motor_right.speed.target_speed_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; + 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; + vTaskDelay(pdMS_TO_TICKS(5000)); + + g_motor_left.speed.target_speed_cms = 20.0f; + g_motor_right.speed.target_speed_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; + vTaskDelay(pdMS_TO_TICKS(5000)); + + set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); + } } @@ -41,21 +57,21 @@ launch() // Left wheel // -// TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; -// xTaskCreate(monitor_wheel_speed_task, -// "monitor_left_wheel_speed_task", -// configMINIMAL_STACK_SIZE, -// (void *)&g_motor_speed_left, -// WHEEL_SPEED_PRIO, -// &h_monitor_left_wheel_speed_task_handle); + TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_wheel_speed_task, + "monitor_left_wheel_speed_task", + configMINIMAL_STACK_SIZE, + (void *)&g_motor_left, + 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_speed_left, -// WHEEL_SPEED_PRIO, -// &h_motor_pid_left_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 // @@ -63,7 +79,7 @@ launch() xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_speed_right, + (void *)&g_motor_right, WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -71,7 +87,7 @@ launch() xTaskCreate(motor_pid_task, "motor_pid_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_speed_right, + (void *)&g_motor_right, WHEEL_SPEED_PRIO, &h_motor_pid_right_task_handle);