add pid anti windup to pid_tuning

diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h
index c4b5e8d27a602da5a80e264079bcacef1eeeec2c..597ca0a0d1f17bc9413a1e3a546fec86381c7cfa 100644 (file)
--- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h
+++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h
@@ -2,7 +2,6 @@
 #define MOTOR_CONTROLLER_H
 
 #include "main.h"
-#include "constants.h"
 
 class MotorController {
  public:
@@ -12,6 +11,7 @@ class MotorController {
   uint16_t dir_pin_;
   TIM_HandleTypeDef *pwm_timer_;
   uint32_t pwm_channel_;
+  int32_t max_dutycycle_;
 
   MotorController(GPIO_TypeDef *sleep_gpio_port, uint16_t sleep_pin,
                   GPIO_TypeDef *dir_gpio_port, uint16_t dir_pin,
@@ -22,6 +22,7 @@ class MotorController {
     this->dir_pin_ = dir_pin;
     this->pwm_timer_ = pwm_timer;
     this->pwm_channel_ = pwm_channel;
+    this->max_dutycycle_ = pwm_timer->Instance->ARR;
   }
 
   void setup() {
@@ -34,8 +35,8 @@ class MotorController {
       HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_SET);
 
       //check if duty_cycle exceeds maximum
-      if (duty_cycle > MAX_DUTY_CYCLE)
-        __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, MAX_DUTY_CYCLE);
+      if (duty_cycle > max_dutycycle_)
+        __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_);
       else
         __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, duty_cycle);
 
@@ -44,8 +45,8 @@ class MotorController {
       HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_RESET);
 
       //check if duty_cycle is lower than minimum
-      if (duty_cycle < -MAX_DUTY_CYCLE)
-        __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, MAX_DUTY_CYCLE);
+      if (duty_cycle < -max_dutycycle_)
+        __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_);
       else
         //invert sign to make duty_cycle positive
       __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, -duty_cycle);

diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h
index 7663f6b45fbdb0693fc68d35b3913f580cf9b383..9fcedc8ee47f619b5cdd7cdada1737a1e52b1de9 100644 (file)
--- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h
+++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h
@@ -22,7 +22,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;
@@ -33,12 +33,12 @@ class Pid {
     this->previous_error_ = 0;
     this->error_sum_ = 0;
 
-    this->min_ = -MAX_DUTY_CYCLE;
-    this->max_ = MAX_DUTY_CYCLE;
+    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;
@@ -49,6 +49,9 @@ class Pid {
     this->previous_error_ = 0;
     this->error_sum_ = 0;
 
+    this->min_ = min;
+    this->max_ = max;
+
   }
 
   void set(float setpoint) {
@@ -70,12 +73,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/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp b/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp
index 519cc94c343426b686e6321cada71f88691fba55..de134c44f58ad2b41c1d7a944ab1a855d1ce732d 100644 (file)
--- a/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp
+++ b/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp
@@ -66,9 +66,12 @@ float left_velocity = 0;
 float right_velocity = 0;\r
 \r
 //PID\r
-Pid left_pid(0, 0, 0);\r
-Pid right_pid(0, 0, 0);\r
-Pid cross_pid(0, 0, 0);\r
+int pid_min = 0;\r
+int pid_max = 0;\r
+\r
+Pid left_pid(0, 0, 0, pid_min, pid_max);\r
+Pid right_pid(0, 0, 0, pid_min, pid_max);\r
+Pid cross_pid(0, 0, 0, pid_min, pid_max);\r
 float left_setpoint;\r
 float right_setpoint;\r
 float cross_setpoint;\r
@@ -80,14 +83,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
@@ -125,14 +126,12 @@ static void MX_NVIC_Init(void);
 /* USER CODE END 0 */\r
 \r
 /**\r
-  * @brief  The application entry point.\r
-  * @retval int\r
-  */\r
-int main(void)\r
-{\r
+ * @brief  The application entry point.\r
+ * @retval int\r
+ */\r
+int main(void) {\r
   /* USER CODE BEGIN 1 */\r
   /* USER CODE END 1 */\r
-  \r
 \r
   /* MCU Configuration--------------------------------------------------------*/\r
 \r
@@ -169,6 +168,9 @@ int main(void)
   left_motor.setup();\r
   right_motor.setup();\r
 \r
+  pid_min = - left_motor.max_dutycycle_;\r
+  pid_max = left_motor.max_dutycycle_;\r
+\r
   left_motor.coast();\r
   right_motor.coast();\r
 \r
@@ -191,81 +193,74 @@ int main(void)
 }\r
 \r
 /**\r
-  * @brief System Clock Configuration\r
-  * @retval None\r
-  */\r
-void SystemClock_Config(void)\r
-{\r
-  RCC_OscInitTypeDef RCC_OscInitStruct = {0};\r
-  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};\r
-  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};\r
+ * @brief System Clock Configuration\r
+ * @retval None\r
+ */\r
+void SystemClock_Config(void) {\r
+  RCC_OscInitTypeDef RCC_OscInitStruct = { 0 };\r
+  RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };\r
+  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = { 0 };\r
 \r
   /** Configure the main internal regulator output voltage \r
-  */\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
-  */\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
   RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;\r
   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;\r
-  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)\r
-  {\r
+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /** Initializes the CPU, AHB and APB busses clocks \r
-  */\r
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK\r
-                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;\r
+   */\r
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK\r
+      | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;\r
   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;\r
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;\r
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;\r
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;\r
 \r
-  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)\r
-  {\r
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART6;\r
   PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;\r
-  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)\r
-  {\r
+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) {\r
     Error_Handler();\r
   }\r
 }\r
 \r
 /**\r
-  * @brief NVIC Configuration.\r
-  * @retval None\r
-  */\r
-static void MX_NVIC_Init(void)\r
-{\r
+ * @brief NVIC Configuration.\r
+ * @retval None\r
+ */\r
+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, 2, 0);\r
-  HAL_NVIC_EnableIRQ(USART6_IRQn);\r
+  HAL_NVIC_EnableIRQ (USART6_IRQn);\r
 }\r
 \r
 /**\r
-  * @brief TIM2 Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_TIM2_Init(void)\r
-{\r
+ * @brief TIM2 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_TIM2_Init(void) {\r
 \r
   /* USER CODE BEGIN TIM2_Init 0 */\r
 \r
   /* USER CODE END TIM2_Init 0 */\r
 \r
-  TIM_Encoder_InitTypeDef sConfig = {0};\r
-  TIM_MasterConfigTypeDef sMasterConfig = {0};\r
+  TIM_Encoder_InitTypeDef sConfig = { 0 };\r
+  TIM_MasterConfigTypeDef sMasterConfig = { 0 };\r
 \r
   /* USER CODE BEGIN TIM2_Init 1 */\r
 \r
@@ -285,14 +280,12 @@ static void MX_TIM2_Init(void)
   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;\r
   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;\r
   sConfig.IC2Filter = 0;\r
-  if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /* USER CODE BEGIN TIM2_Init 2 */\r
@@ -302,19 +295,18 @@ static void MX_TIM2_Init(void)
 }\r
 \r
 /**\r
-  * @brief TIM3 Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_TIM3_Init(void)\r
-{\r
+ * @brief TIM3 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_TIM3_Init(void) {\r
 \r
   /* USER CODE BEGIN TIM3_Init 0 */\r
 \r
   /* USER CODE END TIM3_Init 0 */\r
 \r
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};\r
-  TIM_MasterConfigTypeDef sMasterConfig = {0};\r
+  TIM_ClockConfigTypeDef sClockSourceConfig = { 0 };\r
+  TIM_MasterConfigTypeDef sMasterConfig = { 0 };\r
 \r
   /* USER CODE BEGIN TIM3_Init 1 */\r
 \r
@@ -325,19 +317,16 @@ static void MX_TIM3_Init(void)
   htim3.Init.Period = 159;\r
   htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;\r
   htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;\r
-  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_Base_Init(&htim3) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;\r
-  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /* USER CODE BEGIN TIM3_Init 2 */\r
@@ -347,20 +336,19 @@ static void MX_TIM3_Init(void)
 }\r
 \r
 /**\r
-  * @brief TIM4 Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_TIM4_Init(void)\r
-{\r
+ * @brief TIM4 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_TIM4_Init(void) {\r
 \r
   /* USER CODE BEGIN TIM4_Init 0 */\r
 \r
   /* USER CODE END TIM4_Init 0 */\r
 \r
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};\r
-  TIM_MasterConfigTypeDef sMasterConfig = {0};\r
-  TIM_OC_InitTypeDef sConfigOC = {0};\r
+  TIM_ClockConfigTypeDef sClockSourceConfig = { 0 };\r
+  TIM_MasterConfigTypeDef sMasterConfig = { 0 };\r
+  TIM_OC_InitTypeDef sConfigOC = { 0 };\r
 \r
   /* USER CODE BEGIN TIM4_Init 1 */\r
 \r
@@ -371,35 +359,29 @@ static void MX_TIM4_Init(void)
   htim4.Init.Period = 799;\r
   htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;\r
   htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;\r
-  if (HAL_TIM_Base_Init(&htim4) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_Base_Init(&htim4) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;\r
-  if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
-  if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sConfigOC.OCMode = TIM_OCMODE_PWM1;\r
   sConfigOC.Pulse = 0;\r
   sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;\r
   sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;\r
-  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) {\r
     Error_Handler();\r
   }\r
-  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /* USER CODE BEGIN TIM4_Init 2 */\r
@@ -410,19 +392,18 @@ static void MX_TIM4_Init(void)
 }\r
 \r
 /**\r
-  * @brief TIM5 Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_TIM5_Init(void)\r
-{\r
+ * @brief TIM5 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_TIM5_Init(void) {\r
 \r
   /* USER CODE BEGIN TIM5_Init 0 */\r
 \r
   /* USER CODE END TIM5_Init 0 */\r
 \r
-  TIM_Encoder_InitTypeDef sConfig = {0};\r
-  TIM_MasterConfigTypeDef sMasterConfig = {0};\r
+  TIM_Encoder_InitTypeDef sConfig = { 0 };\r
+  TIM_MasterConfigTypeDef sMasterConfig = { 0 };\r
 \r
   /* USER CODE BEGIN TIM5_Init 1 */\r
 \r
@@ -442,14 +423,12 @@ static void MX_TIM5_Init(void)
   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;\r
   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;\r
   sConfig.IC2Filter = 0;\r
-  if (HAL_TIM_Encoder_Init(&htim5, &sConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_Encoder_Init(&htim5, &sConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /* USER CODE BEGIN TIM5_Init 2 */\r
@@ -459,18 +438,17 @@ static void MX_TIM5_Init(void)
 }\r
 \r
 /**\r
-  * @brief TIM6 Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_TIM6_Init(void)\r
-{\r
+ * @brief TIM6 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_TIM6_Init(void) {\r
 \r
   /* USER CODE BEGIN TIM6_Init 0 */\r
 \r
   /* USER CODE END TIM6_Init 0 */\r
 \r
-  TIM_MasterConfigTypeDef sMasterConfig = {0};\r
+  TIM_MasterConfigTypeDef sMasterConfig = { 0 };\r
 \r
   /* USER CODE BEGIN TIM6_Init 1 */\r
 \r
@@ -480,14 +458,12 @@ static void MX_TIM6_Init(void)
   htim6.Init.CounterMode = TIM_COUNTERMODE_UP;\r
   htim6.Init.Period = 799;\r
   htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;\r
-  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)\r
-  {\r
+  if (HAL_TIM_Base_Init(&htim6) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)\r
-  {\r
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /* USER CODE BEGIN TIM6_Init 2 */\r
@@ -497,12 +473,11 @@ static void MX_TIM6_Init(void)
 }\r
 \r
 /**\r
-  * @brief USART6 Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_USART6_UART_Init(void)\r
-{\r
+ * @brief USART6 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_USART6_UART_Init(void) {\r
 \r
   /* USER CODE BEGIN USART6_Init 0 */\r
 \r
@@ -521,8 +496,7 @@ static void MX_USART6_UART_Init(void)
   huart6.Init.OverSampling = UART_OVERSAMPLING_16;\r
   huart6.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;\r
   huart6.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;\r
-  if (HAL_UART_Init(&huart6) != HAL_OK)\r
-  {\r
+  if (HAL_UART_Init(&huart6) != HAL_OK) {\r
     Error_Handler();\r
   }\r
   /* USER CODE BEGIN USART6_Init 2 */\r
@@ -532,13 +506,12 @@ static void MX_USART6_UART_Init(void)
 }\r
 \r
 /**\r
-  * @brief GPIO Initialization Function\r
-  * @param None\r
-  * @retval None\r
-  */\r
-static void MX_GPIO_Init(void)\r
-{\r
-  GPIO_InitTypeDef GPIO_InitStruct = {0};\r
+ * @brief GPIO Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
+static void MX_GPIO_Init(void) {\r
+  GPIO_InitTypeDef GPIO_InitStruct = { 0 };\r
 \r
   /* GPIO Ports Clock Enable */\r
   __HAL_RCC_GPIOC_CLK_ENABLE();\r
@@ -549,10 +522,10 @@ static void MX_GPIO_Init(void)
   __HAL_RCC_GPIOB_CLK_ENABLE();\r
 \r
   /*Configure GPIO pin Output Level */\r
-  HAL_GPIO_WritePin(GPIOF, dir2_Pin|dir1_Pin, GPIO_PIN_RESET);\r
+  HAL_GPIO_WritePin(GPIOF, dir2_Pin | dir1_Pin, GPIO_PIN_RESET);\r
 \r
   /*Configure GPIO pin Output Level */\r
-  HAL_GPIO_WritePin(GPIOF, sleep2_Pin|sleep1_Pin, GPIO_PIN_SET);\r
+  HAL_GPIO_WritePin(GPIOF, sleep2_Pin | sleep1_Pin, GPIO_PIN_SET);\r
 \r
   /*Configure GPIO pin : user_button_Pin */\r
   GPIO_InitStruct.Pin = user_button_Pin;\r
@@ -579,14 +552,14 @@ static void MX_GPIO_Init(void)
   HAL_GPIO_Init(fault2_GPIO_Port, &GPIO_InitStruct);\r
 \r
   /*Configure GPIO pins : dir2_Pin dir1_Pin */\r
-  GPIO_InitStruct.Pin = dir2_Pin|dir1_Pin;\r
+  GPIO_InitStruct.Pin = dir2_Pin | dir1_Pin;\r
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;\r
   GPIO_InitStruct.Pull = GPIO_NOPULL;\r
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;\r
   HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);\r
 \r
   /*Configure GPIO pins : sleep2_Pin sleep1_Pin */\r
-  GPIO_InitStruct.Pin = sleep2_Pin|sleep1_Pin;\r
+  GPIO_InitStruct.Pin = sleep2_Pin | sleep1_Pin;\r
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;\r
   GPIO_InitStruct.Pull = GPIO_PULLUP;\r
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;\r
@@ -600,7 +573,7 @@ static void MX_GPIO_Init(void)
 \r
   /* EXTI interrupt init*/\r
   HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);\r
-  HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);\r
+  HAL_NVIC_EnableIRQ (EXTI15_10_IRQn);\r
 \r
 }\r
 \r
@@ -658,18 +631,21 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
 \r
   if (input_msg.pid_select == 1) {\r
 \r
-    left_pid.config(input_msg.pid_kp, input_msg.pid_ki, input_msg.pid_kd);\r
+    left_pid.config(input_msg.pid_kp, input_msg.pid_ki, input_msg.pid_kd,\r
+                    pid_min, pid_max);\r
     left_pid.set(input_msg.pid_setpoint_fixed);\r
 \r
   } else if (input_msg.pid_select == 2) {\r
-    right_pid.config(input_msg.pid_kp, input_msg.pid_ki, input_msg.pid_kd);\r
+    right_pid.config(input_msg.pid_kp, input_msg.pid_ki, input_msg.pid_kd,\r
+                     pid_min, pid_max);\r
     right_pid.set(input_msg.pid_setpoint_fixed);\r
 \r
   } else if (input_msg.pid_select == 3) {\r
-    left_pid.config(180, 200, 0);\r
-    right_pid.config(185, 195, 0);\r
+    left_pid.config(180, 200, 0, pid_min, pid_max);\r
+    right_pid.config(185, 195, 0, pid_min, pid_max);\r
 \r
-    cross_pid.config(input_msg.pid_kp, input_msg.pid_ki, input_msg.pid_kd);\r
+    cross_pid.config(input_msg.pid_kp, input_msg.pid_ki, input_msg.pid_kd,\r
+                     pid_min, pid_max);\r
 \r
     odom.UpdateValues(input_msg.pid_setpoint_lin, input_msg.pid_setpoint_ang);\r
 \r
@@ -705,11 +681,10 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
 /* USER CODE END 4 */\r
 \r
 /**\r
-  * @brief  This function is executed in case of error occurrence.\r
-  * @retval None\r
-  */\r
-void Error_Handler(void)\r
-{\r
+ * @brief  This function is executed in case of error occurrence.\r
+ * @retval None\r
+ */\r
+void Error_Handler(void) {\r
   /* USER CODE BEGIN Error_Handler_Debug */\r
   /* User can add his own implementation to report the HAL error return state */\r
 \r