From f284f972d291a48632873c4e828439c3ed868b42 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Wed, 8 Nov 2023 14:00:47 +0800 Subject: [PATCH] PID final --- frtos/config/motor_config.h | 5 ++ frtos/motor/motor_direction.h | 150 +++++++++++++++++----------------- frtos/motor/motor_pid.h | 47 ++++++++++- frtos/motor/motor_speed.h | 51 +++++++++++- frtos/motor/motor_test.c | 7 +- 5 files changed, 180 insertions(+), 80 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 0572d4c..b97c2c7 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -18,6 +18,11 @@ #define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) #define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) +#define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) + +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) + #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 6c31e29..9464cb8 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -80,80 +80,80 @@ turn_right_90() g_motor_right.speed.distance_cm = initial; } -void -spin_left_90() -{ - set_wheel_direction(DIRECTION_LEFT); +//void +//spin_left_90() +//{ +// set_wheel_direction(DIRECTION_LEFT); +// +// set_wheel_speed_synced(90u); +// +// float initial = g_motor_left.speed.distance_cm; +// for (;;) +// { +// if (g_motor_left.speed.distance_cm - initial >= 7.5f) +// { +// set_wheel_speed_synced(0u); +// break; +// } +// vTaskDelay(pdMS_TO_TICKS(5)); +// } +// vTaskDelay(pdMS_TO_TICKS(1000)); +// g_motor_left.speed.distance_cm = initial; +// g_motor_right.speed.distance_cm = initial; +//} - set_wheel_speed_synced(90u); +//void +//spin_right_90() +//{ +// set_wheel_direction(DIRECTION_RIGHT); +// +// set_wheel_speed_synced(90u); +// +// float initial = g_motor_right.speed.distance_cm; +// for (;;) +// { +// if (g_motor_right.speed.distance_cm - initial >= 7.5f) +// { +// set_wheel_speed_synced(0u); +// break; +// } +// vTaskDelay(pdMS_TO_TICKS(5)); +// } +// vTaskDelay(pdMS_TO_TICKS(1000)); +// g_motor_right.speed.distance_cm = initial; +// g_motor_left.speed.distance_cm = initial; +//} - float initial = g_motor_left.speed.distance_cm; - for (;;) - { - if (g_motor_left.speed.distance_cm - initial >= 7.5f) - { - set_wheel_speed_synced(0u); - break; - } - vTaskDelay(pdMS_TO_TICKS(5)); - } - vTaskDelay(pdMS_TO_TICKS(1000)); - g_motor_left.speed.distance_cm = initial; - g_motor_right.speed.distance_cm = initial; -} - -void -spin_right_90() -{ - set_wheel_direction(DIRECTION_RIGHT); - - set_wheel_speed_synced(90u); - - float initial = g_motor_right.speed.distance_cm; - for (;;) - { - if (g_motor_right.speed.distance_cm - initial >= 7.5f) - { - set_wheel_speed_synced(0u); - break; - } - vTaskDelay(pdMS_TO_TICKS(5)); - } - vTaskDelay(pdMS_TO_TICKS(1000)); - g_motor_right.speed.distance_cm = initial; - g_motor_left.speed.distance_cm = initial; -} - -void -spin_to_yaw(float target_yaw) -{ - float initial_yaw = g_direction.yaw; - - // if it will to turn more than 180 degrees, turn the other way - if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) - || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) - { - set_wheel_direction(DIRECTION_LEFT); - } - else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) - || ((target_yaw < initial_yaw) - && (initial_yaw - target_yaw < 180.f))) - { - set_wheel_direction(DIRECTION_RIGHT); - } - - set_wheel_speed_synced(90u); - - g_use_pid = false; - - for (;;) - { - if (initial_yaw == target_yaw) - { - set_wheel_speed_synced(0u); - break; - } - vTaskDelay(pdMS_TO_TICKS(5)); - } - g_use_pid = true; -} \ No newline at end of file +//void +//spin_to_yaw(float target_yaw) +//{ +// float initial_yaw = g_direction.yaw; +// +// // if it will to turn more than 180 degrees, turn the other way +// if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) +// || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) +// { +// set_wheel_direction(DIRECTION_LEFT); +// } +// else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) +// || ((target_yaw < initial_yaw) +// && (initial_yaw - target_yaw < 180.f))) +// { +// set_wheel_direction(DIRECTION_RIGHT); +// } +// +// set_wheel_speed_synced(90u); +// +// g_use_pid = false; +// +// for (;;) +// { +// if (initial_yaw == target_yaw) +// { +// set_wheel_speed_synced(0u); +// break; +// } +// vTaskDelay(pdMS_TO_TICKS(5)); +// } +// g_use_pid = true; +//} \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 1930ed6..cc7ce33 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -36,6 +36,19 @@ compute_pid(float *integral, float *prev_error) return control_signal; } +float +compute_i_controller(float *integral) +{ + float error + = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; + + printf("%f\n", error); + + *integral += error; + + return g_motor_right.pid.ki_value * (*integral); +} + bool repeating_pid_handler(__unused struct repeating_timer *t) { @@ -66,8 +79,38 @@ 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; +} + +bool +repeating_i_handler(__unused struct repeating_timer *t) +{ + static float integral = 0.0f; + + if (!g_use_pid) + { + integral = 0.0f; // reset once disabled + return true; + } + + float control_signal = compute_i_controller(&integral); + + float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; + + if (temp > MAX_PWM_LEVEL) + { + temp = MAX_PWM_LEVEL; + } + + if (temp <= MIN_PWM_LEVEL) + { + temp = MIN_PWM_LEVEL; + } + + set_wheel_speed((uint32_t)temp, &g_motor_right); return true; } \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 230a596..6fc1c58 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -72,4 +72,53 @@ monitor_wheel_speed_task(void *pvParameters) p_motor->speed.current_cms = 0.f; } } -} \ No newline at end of file +} + +void +set_wheel_speed(uint32_t pwm_level, motor_t * motor) +{ + motor->pwm.level = pwm_level; + + pwm_set_chan_level(motor->pwm.slice_num, + motor->pwm.channel, + motor->pwm.level); +} + +///*! +// * @brief Set the speed of the wheels +// * @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); +//} + +///*! +// * @brief Set the distance to travel before stopping, must be called after +// * setting the speed and direction. +// * @param distance_cm distance to travel in cm +// */ +//void +//distance_to_stop(float distance_cm) +//{ +// float initial = g_motor_right.speed.distance_cm; +// +// for (;;) +// { +// if (g_motor_right.speed.distance_cm - initial >= distance_cm) +// { +// set_wheel_speed_synced(0u); +// break; +// } +// vTaskDelay(pdMS_TO_TICKS(10)); +// } +// vTaskDelay(pdMS_TO_TICKS(1000)); +// g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm; +//} diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 8899ebb..5661284 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -10,12 +10,14 @@ motor_control_task(__unused void *params) for (;;) { set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90); + set_wheel_speed(90u, &g_motor_left); + set_wheel_speed(90u, &g_motor_right); vTaskDelay(pdMS_TO_TICKS(10000)); revert_wheel_direction(); - set_wheel_speed_synced(90); + set_wheel_speed(90u, &g_motor_left); + set_wheel_speed(90u, &g_motor_right); vTaskDelay(pdMS_TO_TICKS(10000)); } @@ -37,6 +39,7 @@ launch() // PID timer struct repeating_timer pid_timer; add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); +// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); // Left wheel //