From cd057907a0706a8d6431f3d3ce8c888ec8980d06 Mon Sep 17 00:00:00 2001 From: Federica Di Lauro Date: Fri, 7 Feb 2020 13:46:18 +0100 Subject: [PATCH] add pid antiwindup to otto_controller --- otto_controller/Core/Inc/motor_controller.h | 3 +- otto_controller/Core/Inc/pid.h | 24 +++++++++----- otto_controller/Core/Src/main.cpp | 31 ++++++++++++------- .../otto_pid_tuning/Core/Src/main.cpp | 1 + 4 files changed, 39 insertions(+), 20 deletions(-) diff --git a/otto_controller/Core/Inc/motor_controller.h b/otto_controller/Core/Inc/motor_controller.h index 597ca0a..19d283b 100644 --- a/otto_controller/Core/Inc/motor_controller.h +++ b/otto_controller/Core/Inc/motor_controller.h @@ -22,11 +22,12 @@ class MotorController { this->dir_pin_ = dir_pin; this->pwm_timer_ = pwm_timer; this->pwm_channel_ = pwm_channel; - this->max_dutycycle_ = pwm_timer->Instance->ARR; + this->max_dutycycle_ = 0; } void setup() { HAL_TIM_PWM_Start(pwm_timer_, pwm_channel_); + this->max_dutycycle_ = pwm_timer_->Instance->ARR; } void set_speed(int duty_cycle) { diff --git a/otto_controller/Core/Inc/pid.h b/otto_controller/Core/Inc/pid.h index b351abe..ee067fe 100644 --- a/otto_controller/Core/Inc/pid.h +++ b/otto_controller/Core/Inc/pid.h @@ -20,7 +20,7 @@ class Pid { int min_; int max_; - Pid(float kp, float ki, float kd) { + Pid(float kp, float ki, float kd, int min, int max) { this->kp_ = kp; this->ki_ = ki; this->kd_ = kd; @@ -31,9 +31,12 @@ class Pid { this->previous_error_ = 0; this->error_sum_ = 0; + this->min_ = min; + this->max_ = max; + } - void config(float kp, float ki, float kd) { + void config(float kp, float ki, float kd, int min, int max) { this->kp_ = kp; this->ki_ = ki; this->kd_ = kd; @@ -44,6 +47,9 @@ class Pid { this->previous_error_ = 0; this->error_sum_ = 0; + this->min_ = min; + this->max_ = max; + } void set(float setpoint) { @@ -65,12 +71,16 @@ class Pid { output += (this->error_ - this->previous_error_) * kd_; this->previous_error_ = this->error_; - int integer_output = static_cast (output); + int integer_output = static_cast(output); -// if(integer_output > this->max_) -// integer_output = this->max_; -// else if (integer_output < this->min_) -// integer_output = this->min_; + //anti windup + if (integer_output > this->max_) { + integer_output = this->max_; + this->error_sum_ -= this->error_; + } else if (integer_output < this->min_){ + integer_output = this->min_; + this->error_sum_ -= this->error_; + } return integer_output; diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp index e27ecf7..be12c51 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.cpp @@ -69,10 +69,12 @@ float left_velocity; float right_velocity; //PID +int pid_min = 0; +int pid_max = 0; -Pid left_pid(180, 200, 0); -Pid right_pid(185, 195, 0); -Pid cross_pid(50, 20, 0); +Pid left_pid(180, 200, 0, pid_min, pid_max); +Pid right_pid(185, 195, 0, pid_min, pid_max); +Pid cross_pid(50, 20, 0, pid_min, pid_max); int left_dutycycle; int right_dutycycle; @@ -82,14 +84,12 @@ MotorController right_motor(sleep1_GPIO_Port, sleep1_Pin, dir1_GPIO_Port, dir1_Pin, - &htim4, - TIM_CHANNEL_4); + &htim4, TIM_CHANNEL_4); MotorController left_motor(sleep2_GPIO_Port, sleep2_Pin, dir2_GPIO_Port, dir2_Pin, - &htim4, - TIM_CHANNEL_3); + &htim4, TIM_CHANNEL_3); //Communication uint8_t *tx_buffer; @@ -157,6 +157,14 @@ int main(void) { left_motor.setup(); right_motor.setup(); + //right and left motors have the same parameters + pid_min = -left_motor.max_dutycycle_; + pid_max = left_motor.max_dutycycle_; + + left_pid.config(180, 200, 0, pid_min, pid_max); + right_pid.config(185, 195, 0, pid_min, pid_max); + cross_pid.config(50, 20, 0, pid_min, pid_max); + left_motor.coast(); right_motor.coast(); @@ -190,8 +198,7 @@ void SystemClock_Config(void) { /** Configure the main internal regulator output voltage */ __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3); - /** Initializes the CPU, AHB and APB busses clocks + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);/** Initializes the CPU, AHB and APB busses clocks */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; RCC_OscInitStruct.HSIState = RCC_HSI_ON; @@ -226,13 +233,13 @@ void SystemClock_Config(void) { static void MX_NVIC_Init(void) { /* TIM3_IRQn interrupt configuration */ HAL_NVIC_SetPriority(TIM3_IRQn, 2, 1); - HAL_NVIC_EnableIRQ(TIM3_IRQn); + HAL_NVIC_EnableIRQ (TIM3_IRQn); /* TIM6_DAC_IRQn interrupt configuration */ HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 2); - HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn); + HAL_NVIC_EnableIRQ (TIM6_DAC_IRQn); /* USART6_IRQn interrupt configuration */ HAL_NVIC_SetPriority(USART6_IRQn, 1, 0); - HAL_NVIC_EnableIRQ(USART6_IRQn); + HAL_NVIC_EnableIRQ (USART6_IRQn); } /* USER CODE BEGIN 4 */ diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp b/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp index de134c4..84465c9 100644 --- a/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp +++ b/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp @@ -168,6 +168,7 @@ int main(void) { left_motor.setup(); right_motor.setup(); + //right and left motors have the same parameters pid_min = - left_motor.max_dutycycle_; pid_max = left_motor.max_dutycycle_; -- 2.52.0