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