add pid antiwindup to otto_controller

diff --git a/otto_controller/Core/Inc/motor_controller.h b/otto_controller/Core/Inc/motor_controller.h
index 597ca0a0d1f17bc9413a1e3a546fec86381c7cfa..19d283bbafe87c5a02be724dc4a4c4aaa8d3eaa0 100644 (file)
--- 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 b351abe8171fc5b7d3a123e8c1c1024d681f9d52..ee067fe7fddf128ded67aa9dd3c05f7437ba13eb 100644 (file)
--- 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<int> (output);
+    int integer_output = static_cast<int>(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 e27ecf7bb1c042cc2ef3d263f2bfb4abf803280c..be12c515172e7bc275fb5cad2805c6eac61f2291 100644 (file)
--- a/otto_controller/Core/Src/main.cpp
+++ b/otto_controller/Core/Src/main.cpp
@@ -69,10 +69,12 @@ float left_velocity;
 float right_velocity;\r
 \r
 //PID\r
+int pid_min = 0;\r
+int pid_max = 0;\r
 \r
-Pid left_pid(180, 200, 0);\r
-Pid right_pid(185, 195, 0);\r
-Pid cross_pid(50, 20, 0);\r
+Pid left_pid(180, 200, 0, pid_min, pid_max);\r
+Pid right_pid(185, 195, 0, pid_min, pid_max);\r
+Pid cross_pid(50, 20, 0, pid_min, pid_max);\r
 \r
 int left_dutycycle;\r
 int right_dutycycle;\r
@@ -82,14 +84,12 @@ MotorController right_motor(sleep1_GPIO_Port,
 sleep1_Pin,\r
                             dir1_GPIO_Port,\r
                             dir1_Pin,\r
-                            &htim4,\r
-                            TIM_CHANNEL_4);\r
+                            &htim4, TIM_CHANNEL_4);\r
 MotorController left_motor(sleep2_GPIO_Port,\r
 sleep2_Pin,\r
                            dir2_GPIO_Port,\r
                            dir2_Pin,\r
-                           &htim4,\r
-                           TIM_CHANNEL_3);\r
+                           &htim4, TIM_CHANNEL_3);\r
 \r
 //Communication\r
 uint8_t *tx_buffer;\r
@@ -157,6 +157,14 @@ int main(void) {
   left_motor.setup();\r
   right_motor.setup();\r
 \r
+  //right and left motors have the same parameters\r
+  pid_min = -left_motor.max_dutycycle_;\r
+  pid_max = left_motor.max_dutycycle_;\r
+\r
+  left_pid.config(180, 200, 0, pid_min, pid_max);\r
+  right_pid.config(185, 195, 0, pid_min, pid_max);\r
+  cross_pid.config(50, 20, 0, pid_min, pid_max);\r
+\r
   left_motor.coast();\r
   right_motor.coast();\r
 \r
@@ -190,8 +198,7 @@ void SystemClock_Config(void) {
   /** Configure the main internal regulator output voltage \r
    */\r
   __HAL_RCC_PWR_CLK_ENABLE();\r
-  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);\r
-  /** Initializes the CPU, AHB and APB busses clocks \r
+  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);/** Initializes the CPU, AHB and APB busses clocks\r
    */\r
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;\r
   RCC_OscInitStruct.HSIState = RCC_HSI_ON;\r
@@ -226,13 +233,13 @@ void SystemClock_Config(void) {
 static void MX_NVIC_Init(void) {\r
   /* TIM3_IRQn interrupt configuration */\r
   HAL_NVIC_SetPriority(TIM3_IRQn, 2, 1);\r
-  HAL_NVIC_EnableIRQ(TIM3_IRQn);\r
+  HAL_NVIC_EnableIRQ (TIM3_IRQn);\r
   /* TIM6_DAC_IRQn interrupt configuration */\r
   HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 2);\r
-  HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);\r
+  HAL_NVIC_EnableIRQ (TIM6_DAC_IRQn);\r
   /* USART6_IRQn interrupt configuration */\r
   HAL_NVIC_SetPriority(USART6_IRQn, 1, 0);\r
-  HAL_NVIC_EnableIRQ(USART6_IRQn);\r
+  HAL_NVIC_EnableIRQ (USART6_IRQn);\r
 }\r
 \r
 /* USER CODE BEGIN 4 */\r

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 de134c44f58ad2b41c1d7a944ab1a855d1ce732d..84465c9a1154d20e29f67fc5156ea2312295b190 100644 (file)
--- 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();\r
   right_motor.setup();\r
 \r
+  //right and left motors have the same parameters\r
   pid_min = - left_motor.max_dutycycle_;\r
   pid_max = left_motor.max_dutycycle_;\r
 \r