devbox
盐析白兔
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

电机应用-直流有刷电机多环控制实现_位置环串级pid

作者：盐析白兔 | 2024-08-11 08:31:39

赞

踩

位置环串级pid

目录

直流有刷电机多环控制实现

硬件设计

直流电机三环（速度环、电流环、位置环）串级PID控制-位置式PID

编程要点

配置ADC可读取电流值

配置基本定时器6产生定时中断读取当前电路中驱动电机的电流值并执行PID运算

配置定时器1输出PWM控制电机

配置定时器3读取编码器的计数值

ADC数据处理

编写位置式PID算法

直流有刷电机多环控制实现

外环的输出会作为内环的输入。外环一般是最终要控制的效果， 

硬件设计

可选：L298N电机驱动板、野火MOS搭建的驱动板。

直流电机三环（速度环、电流环、位置环）串级PID控制-位置式PID

编程要点

配置ADC可读取电流值。

配置基本定时器TIM6产生定时中断执行PID运算。

配置高级定时器TIM1输出PWM控制电机。

配置通用定时器TIM3读取编码器的计数值。

ADC数据处理。

编写位置式PID算法。

编写位置环、速度环、电流环控制函数。

增加上位机曲线观察相关代码。

编写按键控制代码。

配置ADC可读取电流值
  1. #define VBUS_MAX      		14    // 电压最大值
  2. #define VBUS_MIN      		10    // 电压最小值
  3. 			
  4. #define VBUS_HEX_MAX  		((VBUS_MAX/37.0+1.24)/VREF*65536)    // 电压最大值（测量电压是电源电压的1/37）
  5. #define VBUS_HEX_MIN  		((VBUS_MIN/37.0+1.24)/VREF*65536)    // 电压最小值（测量电压是电源电压的1/37）
  6.  
  7. __IO uint16_t ADC_ConvertedValue;
  8. DMA_HandleTypeDef DMA_Init_Handle;
  9. ADC_HandleTypeDef ADC_Handle;
  10.  
  11. static uint16_t adc_buff[1024];
  12. static uint16_t vbus_adc_mean = 0;  	// 电源电压 ADC 采样结果平均值
  13. static uint32_t adc_mean_sum = 0;   	// 平均值累加
  14. static uint32_t adc_mean_count = 0; 	// 累加计数
  15.  
  16. /**
  17.   * @brief  电流采集初始化
  18.   * @param  无
  19.   * @retval 无
  20.   */
  21. void ADC_Init(void)
  22. {
  23. 	GPIO_InitTypeDef GPIO_InitStructure;
  24. 	
  25.     __GPIOB_CLK_ENABLE();
  26. 	__DMA2_CLK_ENABLE();
  27. 	__ADC1_CLK_ENABLE();
  28.  
  29. 	// PB1--电流
  30.     GPIO_InitStructure.Pin = GPIO_PIN_1;
  31.     GPIO_InitStructure.Mode = GPIO_MODE_ANALOG;
  32.     GPIO_InitStructure.Pull = GPIO_NOPULL;
  33.     HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
  34.  
  35. 	// PB0--电压
  36.     GPIO_InitStructure.Pin = GPIO_PIN_0;
  37.     HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
  38. 	
  39. 	// ADC1使用DMA2，数据流0，通道0，这个是手册固定死的
  40. 	DMA_Init_Handle.Instance 					= DMA2_Stream0;
  41.     DMA_Init_Handle.Init.Direction 				= DMA_PERIPH_TO_MEMORY;
  42.     DMA_Init_Handle.Init.PeriphInc 				= DMA_PINC_DISABLE;
  43.     DMA_Init_Handle.Init.MemInc 				= DMA_MINC_ENABLE;
  44.     DMA_Init_Handle.Init.PeriphDataAlignment 	= DMA_PDATAALIGN_HALFWORD;
  45.     DMA_Init_Handle.Init.MemDataAlignment 		= DMA_MDATAALIGN_HALFWORD;
  46.     DMA_Init_Handle.Init.Mode 					= DMA_CIRCULAR;
  47.     DMA_Init_Handle.Init.Priority 				= DMA_PRIORITY_HIGH;
  48.     DMA_Init_Handle.Init.FIFOMode 				= DMA_FIFOMODE_DISABLE;
  49.     DMA_Init_Handle.Init.FIFOThreshold 			= DMA_FIFO_THRESHOLD_HALFFULL;
  50.     DMA_Init_Handle.Init.MemBurst 				= DMA_MBURST_SINGLE;
  51.     DMA_Init_Handle.Init.PeriphBurst 			= DMA_PBURST_SINGLE;
  52.     // 选择 DMA 通道，通道存在于流中
  53.     DMA_Init_Handle.Init.Channel 				= DMA_CHANNEL_0;
  54.     //初始化DMA流，流相当于一个大的管道，管道里面有很多通道
  55.     HAL_DMA_Init(&DMA_Init_Handle);
  56.  
  57.     __HAL_LINKDMA(&ADC_Handle, DMA_Handle, DMA_Init_Handle);
  58. 	
  59. 	ADC_Handle.Instance 					= ADC1;
  60.     ADC_Handle.Init.ClockPrescaler 			= ADC_CLOCKPRESCALER_PCLK_DIV4;
  61.     ADC_Handle.Init.Resolution 				= ADC_RESOLUTION_12B;
  62.     ADC_Handle.Init.ScanConvMode 			= ENABLE;
  63.     ADC_Handle.Init.ContinuousConvMode 		= ENABLE;
  64.     ADC_Handle.Init.DiscontinuousConvMode 	= DISABLE;
  65.     ADC_Handle.Init.NbrOfDiscConversion   	= 0;
  66.     ADC_Handle.Init.ExternalTrigConvEdge 	= ADC_EXTERNALTRIGCONVEDGE_NONE;
  67.     ADC_Handle.Init.ExternalTrigConv 		= ADC_SOFTWARE_START;
  68.     ADC_Handle.Init.DataAlign 				= ADC_DATAALIGN_LEFT;
  69.     ADC_Handle.Init.NbrOfConversion 		= 2;
  70.     ADC_Handle.Init.DMAContinuousRequests 	= ENABLE;
  71.     ADC_Handle.Init.EOCSelection          	= ADC_EOC_SINGLE_CONV;
  72.     HAL_ADC_Init(&ADC_Handle);
  73. 	
  74. 	ADC_ChannelConfTypeDef ADC_Config;
  75.  
  76.     ADC_Config.Channel      = ADC_CHANNEL_9;
  77.     ADC_Config.Rank         = 1;
  78.     ADC_Config.SamplingTime = ADC_SAMPLETIME_3CYCLES;
  79.     ADC_Config.Offset       = 0;
  80.     HAL_ADC_ConfigChannel(&ADC_Handle, &ADC_Config);
  81.  
  82.     /** Configure the analog watchdog
  83.     */
  84.     ADC_AnalogWDGConfTypeDef AnalogWDGConfig = {0};
  85.  
  86.     AnalogWDGConfig.WatchdogMode 	= ADC_ANALOGWATCHDOG_SINGLE_REG;
  87.     AnalogWDGConfig.HighThreshold 	= VBUS_HEX_MAX;
  88.     AnalogWDGConfig.LowThreshold 	= VBUS_HEX_MIN;
  89.     AnalogWDGConfig.Channel 		= ADC_CHANNEL_8;
  90.     AnalogWDGConfig.ITMode 			= ENABLE;
  91.  
  92.     if (HAL_ADC_AnalogWDGConfig(&ADC_Handle, &AnalogWDGConfig) != HAL_OK)
  93.     {
  94.         while (1);
  95.     }
  96.  
  97.     /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
  98.     */
  99.     ADC_Config.Channel 		= ADC_CHANNEL_8;
  100.     ADC_Config.Rank 		= 2;
  101.     ADC_Config.SamplingTime = ADC_SAMPLETIME_3CYCLES;
  102.     ADC_Config.Offset       = 0;
  103.  
  104.     if (HAL_ADC_ConfigChannel(&ADC_Handle, &ADC_Config) != HAL_OK)
  105.     {
  106.         while (1);
  107.     }
  108.  
  109.     // 外设中断优先级配置和使能中断配置
  110.     HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 4, 0);
  111.     HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
  112.  
  113.     HAL_NVIC_SetPriority(ADC_IRQn, 3, 0);
  114.     HAL_NVIC_EnableIRQ(ADC_IRQn);
  115.  
  116.     HAL_ADC_Start_DMA(&ADC_Handle, (uint32_t *)&adc_buff, 1024);
  117. }

配置基本定时器6产生定时中断读取当前电路中驱动电机的电流值并执行PID运算
  1. TIM_HandleTypeDef TIM_TimeBaseStructure;
  2.  
  3. /**
  4.   * @brief  初始化基本定时器定时，默认50ms产生一次中断
  5.   * @param  无
  6.   * @retval 无
  7.   */
  8. void TIMx_Configuration(void)
  9. {
  10.     HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 1, 3);
  11.     HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
  12. 	
  13. 	__TIM6_CLK_ENABLE();
  14. 	
  15. 	TIM_TimeBaseStructure.Instance = TIM6;
  16.     TIM_TimeBaseStructure.Init.Period = 50 * 50 - 1;
  17.     TIM_TimeBaseStructure.Init.Prescaler = 1680 - 1;
  18.     TIM_TimeBaseStructure.Init.CounterMode = TIM_COUNTERMODE_UP;
  19.     TIM_TimeBaseStructure.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  20.     HAL_TIM_Base_Init(&TIM_TimeBaseStructure);
  21.  
  22.     // 开启定时器更新中断
  23.     HAL_TIM_Base_Start_IT(&TIM_TimeBaseStructure);
  24. 	
  25.     uint32_t temp = (__HAL_TIM_GET_AUTORELOAD(&TIM_TimeBaseStructure) + 1) / 50.0;  // 计算周期，单位ms
  26.     set_computer_value(SEND_PERIOD_CMD, CURVES_CH1, &temp, 1);  					// 给通道 1 发送目标值
  27. }
  28.  
  29. /**
  30.   * @brief  定时器更新事件回调函数
  31.   * @param  无
  32.   * @retval 无
  33.   */
  34. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  35. {
  36.     if (htim == (&TIM_TimeBaseStructure))
  37.     {
  38.         motor_pid_control();	// 每50ms执行一次PID运算
  39.     }
  40. }

配置定时器1输出PWM控制电机
  1. TIM_HandleTypeDef  DCM_TimeBaseStructure;
  2.  
  3. /**
  4.   * @brief  初始化控制通用定时器
  5.   * @param  无
  6.   * @retval 无
  7.   */
  8. void Motor_TIMx_Configuration(void)
  9. {
  10.     GPIO_InitTypeDef GPIO_InitStruct;
  11.     TIM_OC_InitTypeDef  TIM_OCInitStructure;
  12.  
  13. 	__HAL_RCC_GPIOA_CLK_ENABLE();
  14.     __TIM1_CLK_ENABLE();
  15.  
  16. 	// PA8--PWM_TIM_CH1
  17. 	GPIO_InitStruct.Pin 		= GPIO_PIN_8;
  18.     GPIO_InitStruct.Mode 		= GPIO_MODE_AF_PP;
  19.     GPIO_InitStruct.Speed 		= GPIO_SPEED_FREQ_HIGH;
  20.     GPIO_InitStruct.Alternate 	= PWM_TIM_GPIO_AF;
  21.     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
  22.  
  23. 	// PA9--PWM_TIM_CH2
  24.     GPIO_InitStruct.Pin 		= GPIO_PIN_9;
  25.     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
  26.  
  27. 	// TIM1 66.7us一次周期
  28.     DCM_TimeBaseStructure.Instance = TIM1;
  29.     DCM_TimeBaseStructure.Init.Period = 5600 - 1;
  30.     DCM_TimeBaseStructure.Init.Prescaler = 1 - 1;
  31.     DCM_TimeBaseStructure.Init.CounterMode = TIM_COUNTERMODE_UP;
  32.     DCM_TimeBaseStructure.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  33.     HAL_TIM_PWM_Init(&DCM_TimeBaseStructure);
  34.  
  35.     /*PWM模式配置*/
  36.     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
  37.     TIM_OCInitStructure.Pulse = 0;
  38.     TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
  39.     TIM_OCInitStructure.OCNPolarity = TIM_OCNPOLARITY_HIGH;
  40.     TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
  41.     TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  42.  
  43.     /*配置PWM通道*/
  44.     HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_1);
  45.     /*开始输出PWM*/
  46.     HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_1);
  47.  
  48.     /*配置PWM通道*/
  49.     HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_2);
  50.     /*开始输出PWM*/
  51.     HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_2);
  52. }
  53.  
  54. /**
  55.   * @brief  设置TIM通道的占空比
  56. 	* @param  channel		通道	（1,2,3,4）
  57. 	* @param  compare		占空比
  58. 	*	@note 	无
  59.   * @retval 无
  60.   */
  61. void TIM1_SetPWM_pulse(uint32_t channel, int compare)
  62. {
  63.     switch (channel)
  64.     {
  65.         case TIM_CHANNEL_1:
  66.             __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_1, compare);
  67.             break;
  68.  
  69.         case TIM_CHANNEL_2:
  70.             __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_2, compare);
  71.             break;
  72.  
  73.         case TIM_CHANNEL_3:
  74.             __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_3, compare);
  75.             break;
  76.  
  77.         case TIM_CHANNEL_4:
  78.             __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_4, compare);
  79.             break;
  80.     }
  81. }

配置定时器3读取编码器的计数值
  1. TIM_HandleTypeDef  DCM_TimeBaseStructure;
  2.  
  3. /**
  4.   * @brief  初始化控制通用定时器
  5.   * @param  无
  6.   * @retval 无
  7.   */
  8. void Motor_TIMx_Configuration(void)
  9. {
  10.     GPIO_InitTypeDef GPIO_InitStruct;
  11.     TIM_OC_InitTypeDef  TIM_OCInitStructure;
  12.  
  13. 	__HAL_RCC_GPIOA_CLK_ENABLE();
  14.     __TIM1_CLK_ENABLE();
  15.  
  16. 	// PA8--PWM_TIM_CH1
  17. 	GPIO_InitStruct.Pin 		= GPIO_PIN_8;
  18.     GPIO_InitStruct.Mode 		= GPIO_MODE_AF_PP;
  19.     GPIO_InitStruct.Speed 		= GPIO_SPEED_FREQ_HIGH;
  20.     GPIO_InitStruct.Alternate 	= PWM_TIM_GPIO_AF;
  21.     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
  22.  
  23. 	// PA9--PWM_TIM_CH2
  24.     GPIO_InitStruct.Pin 		= GPIO_PIN_9;
  25.     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
  26.  
  27. 	// TIM1 66.7us一次周期
  28.     DCM_TimeBaseStructure.Instance = TIM1;
  29.     DCM_TimeBaseStructure.Init.Period = 5600 - 1;
  30.     DCM_TimeBaseStructure.Init.Prescaler = 1 - 1;
  31.     DCM_TimeBaseStructure.Init.CounterMode = TIM_COUNTERMODE_UP;
  32.     DCM_TimeBaseStructure.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  33.     HAL_TIM_PWM_Init(&DCM_TimeBaseStructure);
  34.  
  35.     /*PWM模式配置*/
  36.     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
  37.     TIM_OCInitStructure.Pulse = 0;
  38.     TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
  39.     TIM_OCInitStructure.OCNPolarity = TIM_OCNPOLARITY_HIGH;
  40.     TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
  41.     TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  42.  
  43.     /*配置PWM通道*/
  44.     HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_1);
  45.     /*开始输出PWM*/
  46.     HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_1);
  47.  
  48.     /*配置PWM通道*/
  49.     HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_2);
  50.     /*开始输出PWM*/
  51.     HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_2);
  52. }
  53.  
  54. /**
  55.   * @brief  定时器更新事件回调函数
  56.   * @param  无
  57.   * @retval 无
  58.   */
  59. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  60. {
  61.     if (htim == (&TIM_EncoderHandle))
  62.     {
  63.         /* 判断当前计数器计数方向 */
  64.         if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&TIM_EncoderHandle))
  65.             /* 下溢 */
  66.         {
  67.             Encoder_Overflow_Count--;
  68.         }
  69.         else
  70.             /* 上溢 */
  71.         {
  72.             Encoder_Overflow_Count++;
  73.         }
  74.     }
  75. }

ADC数据处理
  1. static uint16_t flag_num = 0;
  2. /**
  3.   * @brief  常规转换在非阻塞模式下完成回调
  4.   * @param  hadc: ADC  句柄.
  5.   * @retval 无
  6.   */
  7. void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc)
  8. {
  9.     uint32_t adc_mean = 0;
  10.  
  11.     HAL_ADC_Stop_DMA(hadc);       // 停止 ADC 采样，处理完一次数据在继续采样
  12.  
  13.     /* 计算电流通道采样的平均值 */
  14.     for (uint32_t count = 0; count < 1024; count += 2)
  15.     {
  16.         adc_mean += (uint32_t)adc_buff[count];
  17.     }
  18.  
  19.     adc_mean_sum += adc_mean / (1024 / 2);    // 保存平均值
  20.     adc_mean_count++;
  21.  
  22.     adc_mean = 0;
  23.  
  24.     /* 计算电压通道采样的平均值 */
  25.     for (uint32_t count = 1; count < 1024; count += 2)
  26.     {
  27.         adc_mean += (uint32_t)adc_buff[count];
  28.     }
  29.  
  30.     vbus_adc_mean = adc_mean / (1024 / 2);    // 保存平均值
  31.  
  32.     HAL_ADC_Start_DMA(&ADC_Handle, (uint32_t *)&adc_buff, 1024); // 开始 ADC 采样
  33. }
  34.  
  35. /**
  36.   * @brief  在非阻塞模式模拟看门狗回调
  37.   * @param  hadc: ADC  句柄.
  38.   * @retval 无
  39.   */
  40. void HAL_ADC_LevelOutOfWindowCallback(ADC_HandleTypeDef *hadc)
  41. {
  42.     float temp_adc;
  43.  
  44.     flag_num++;     	// 电源电压超过阈值电压
  45.  
  46.     temp_adc = get_vbus_val();
  47.  
  48.     if (temp_adc > VBUS_MIN && temp_adc < VBUS_MAX)
  49.     {
  50.         flag_num = 0;
  51.     }
  52.  
  53.     if (flag_num > 10) 	// 电源电压超过阈值电压10次
  54.     {
  55.         set_motor_disable();
  56.         flag_num = 0;
  57.         printf("电源电压超过限制！请检查原因，复位开发板在试！\r\n");
  58.  
  59.         while (1);
  60.     }
  61. }
  62.  
  63. /**
  64.   * @brief  获取电流值（应定时调用）
  65.   * @param  无
  66.   * @retval 转换得到的电流值
  67.   */
  68. int32_t get_curr_val(void)
  69. {
  70.     static uint8_t flag = 0;
  71.     static uint32_t adc_offset = 0;    				// 偏置电压
  72.     int16_t curr_adc_mean = 0;         				// 电流 ACD 采样结果平均值
  73.  
  74.     curr_adc_mean = adc_mean_sum / adc_mean_count;  // 保存平均值
  75.  
  76.     adc_mean_count = 0;
  77.     adc_mean_sum = 0;
  78.  
  79.     if (flag < 17 && is_motor_en == 0)				//	仅在电机未启动时记录
  80.     {
  81.         adc_offset = curr_adc_mean;    				// 多次记录偏置电压，待系统稳定偏置电压才为有效值
  82.         flag += 1;
  83.     }
  84.  
  85.     if (curr_adc_mean >= adc_offset)
  86.     {
  87.         curr_adc_mean -= adc_offset;                // 减去偏置电压
  88.     }
  89.     else
  90.     {
  91.         curr_adc_mean = 0;
  92.     }
  93.  
  94. 	float vdc = (float)curr_adc_mean/(float)65536 * 3.3f;	// 获取电压值
  95.     return (float)vdc / 8.0f / 0.02f * 1000.0f;
  96. }
  97.  
  98. /**
  99.   * @brief  获取电源电压值
  100.   * @param  无
  101.   * @retval 转换得到的电流值
  102.   */
  103. float get_vbus_val(void)
  104. {
  105.     float vdc = (float)vbus_adc_mean/(float)65536 * 3.3f;	// 获取电压值
  106.     return ((float)vdc - (float)1.24) * (float)37.0;		// 电源电压值（测量电压是电源电压的1/37）
  107. }

编写位置式PID算法
  1. typedef struct
  2. {
  3.     float target_val;	// 目标值
  4.     float actual_val;	// 实际值
  5.     float err;       	// 定义偏差值
  6.     float err_last;  	// 定义上一个偏差值
  7.     float Kp,Ki,Kd;  	// 定义比例、积分、微分系数
  8.     float integral;  	// 定义积分值
  9. }_pid;
  10. _pid pid_location;		// 位置环控制
  11. _pid pid_curr;			// 电流环控制
  12. _pid pid_speed;			// 速度环控制
  13.  
  14. /**
  15.   * @brief  PID参数初始化
  16. 	*	@note 	无
  17.   * @retval 无
  18.   */
  19. void PID_param_init(void)
  20. {
  21.     /* 位置相关初始化参数 */
  22.     pid_location.target_val = 0.0;
  23.     pid_location.actual_val = 0.0;
  24.     pid_location.err 		= 0.0;
  25.     pid_location.err_last 	= 0.0;
  26.     pid_location.integral 	= 0.0;
  27.     pid_location.Kp 		= 0.011;
  28.     pid_location.Ki 		= 0.0018;
  29.     pid_location.Kd 		= 0.0;
  30.  
  31.     /* 速度相关初始化参数 */
  32.     pid_speed.target_val 	= 100.0;
  33.     pid_speed.actual_val 	= 0.0;
  34.     pid_speed.err 			= 0.0;
  35.     pid_speed.err_last 		= 0.0;
  36.     pid_speed.integral 		= 0.0;
  37.     pid_speed.Kp 			= 2.0;
  38.     pid_speed.Ki 			= 0.02;
  39.     pid_speed.Kd 			= 0.00;
  40.  
  41.     /* 电流相关初始化参数 */
  42.     pid_curr.target_val 	= 80.0;
  43.     pid_curr.actual_val 	= 0.0;
  44.     pid_curr.err 			= 0.0;
  45.     pid_curr.err_last 		= 0.0;
  46.     pid_curr.integral 		= 0.0;
  47.     pid_curr.Kp 			= 0.0;
  48.     pid_curr.Ki 			= 3.5;
  49.     pid_curr.Kd 			= 0.00;
  50.  
  51.     float pid_temp[3] = {pid_location.Kp, pid_location.Ki, pid_location.Kd};
  52.     set_computer_value(SEND_P_I_D_CMD, CURVES_CH1, pid_temp, 3);     // 给通道 1 发送 P I D 值
  53.  
  54.     pid_temp[0] = pid_speed.Kp;
  55.     pid_temp[1] = pid_speed.Ki;
  56.     pid_temp[2] = pid_speed.Kd;
  57.     set_computer_value(SEND_P_I_D_CMD, CURVES_CH2, pid_temp, 3);     // 给通道 2 发送 P I D 值
  58.  
  59.     pid_temp[0] = pid_curr.Kp;
  60.     pid_temp[1] = pid_curr.Ki;
  61.     pid_temp[2] = pid_curr.Kd;
  62.     set_computer_value(SEND_P_I_D_CMD, CURVES_CH3, pid_temp, 3);     // 给通道 3 发送 P I D 值
  63. }
  64.  
  65. /**
  66.   * @brief  设置目标值
  67.   * @param  val		目标值
  68. 	*	@note 	无
  69.   * @retval 无
  70.   */
  71. void set_pid_target(_pid *pid, float temp_val)
  72. {
  73.     pid->target_val = temp_val;    // 设置当前的目标值
  74. }
  75.  
  76. /**
  77.   * @brief  获取目标值
  78.   * @param  无
  79. 	*	@note 	无
  80.   * @retval 目标值
  81.   */
  82. float get_pid_target(_pid *pid)
  83. {
  84.     return pid->target_val;    // 设置当前的目标值
  85. }
  86.  
  87. /**
  88.   * @brief  设置比例、积分、微分系数
  89.   * @param  p：比例系数 P
  90.   * @param  i：积分系数 i
  91.   * @param  d：微分系数 d
  92. 	*	@note 	无
  93.   * @retval 无
  94.   */
  95. void set_p_i_d(_pid *pid, float p, float i, float d)
  96. {
  97.     pid->Kp = p;    // 设置比例系数 P
  98.     pid->Ki = i;    // 设置积分系数 I
  99.     pid->Kd = d;    // 设置微分系数 D
  100. }
  101.  
  102. /**
  103.   * @brief  位置环位置式PID算法实现
  104.   * @param  actual_val:实际值
  105.   *	@note 	无
  106.   * @retval 通过PID计算后的输出
  107.   */
  108. float location_pid_realize(_pid *pid, float actual_val)
  109. {
  110.     /*计算目标值与实际值的误差*/
  111.     pid->err = pid->target_val - actual_val;
  112.  
  113.     /* 限定闭环死区 */
  114.     if ((pid->err >= -40) && (pid->err <= 40))
  115.     {
  116.         pid->err = 0;
  117.         pid->integral = 0;
  118.     }
  119.  
  120.     /* 积分分离，偏差较大时去掉积分作用 */
  121.     if (pid->err > -1500 && pid->err < 1500)
  122.     {
  123.         pid->integral += pid->err;    // 误差累积
  124.  
  125.         /* 限定积分范围，防止积分饱和 */
  126.         if (pid->integral > 4000)
  127.         {
  128.             pid->integral = 4000;
  129.         }
  130.         else if (pid->integral < -4000)
  131.         {
  132.             pid->integral = -4000;
  133.         }
  134.     }
  135.  
  136.     /*PID算法实现*/
  137.     pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);
  138.  
  139.     /*误差传递*/
  140.     pid->err_last = pid->err;
  141.  
  142.     /*返回当前实际值*/
  143.     return pid->actual_val;
  144. }
  145.  
  146. /**
  147.   * @brief  速度环位置式PID算法实现
  148.   * @param  actual_val:实际值
  149.   *	@note 	无
  150.   * @retval 通过PID计算后的输出
  151.   */
  152. float speed_pid_realize(_pid *pid, float actual_val)
  153. {
  154.     /*计算目标值与实际值的误差*/
  155.     pid->err = pid->target_val - actual_val;
  156.  
  157.     if ((pid->err < 0.2f) && (pid->err > -0.2f))
  158.     {
  159.         pid->err = 0.0f;
  160.     }
  161.  
  162.     pid->integral += pid->err;    // 误差累积
  163.  
  164.     /*PID算法实现*/
  165.     pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);
  166.  
  167.     /*误差传递*/
  168.     pid->err_last = pid->err;
  169.  
  170.     /*返回当前实际值*/
  171.     return pid->actual_val;
  172. }
  173.  
  174. /**
  175.   * @brief  电流环位置式PID算法实现
  176.   * @param  actual_val:实际值
  177. 	*	@note 	无
  178.   * @retval 通过PID计算后的输出
  179.   */
  180. float curr_pid_realize(_pid *pid, float actual_val)
  181. {
  182.     /*计算目标值与实际值的误差*/
  183.     pid->err = pid->target_val - actual_val;
  184.  
  185.     pid->integral += pid->err;    // 误差累积
  186.  
  187.     /* 限定积分范围，防止积分饱和 */
  188.     if (pid->integral > 2000)
  189.     {
  190.         pid->integral = 2000;
  191.     }
  192.     else if (pid->integral < -2000)
  193.     {
  194.         pid->integral = -2000;
  195.     }
  196.  
  197.     /*PID算法实现*/
  198.     pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);
  199.  
  200.     /*误差传递*/
  201.     pid->err_last = pid->err;
  202.  
  203.     /*返回当前实际值*/
  204.     return pid->actual_val;
  205. }
  206.  
  207. #define TARGET_CURRENT_MAX    200    // 目标电流的最大值 mA
  208. #define TARGET_SPEED_MAX      200    // 目标速度的最大值 r/m
  209.  
  210. /**
  211.   * @brief  电机位置式 PID 控制实现(定时调用)
  212.   * @param  无
  213.   * @retval 无
  214.   */
  215. void motor_pid_control(void)
  216. {
  217.     static uint32_t louter_ring_timer = 0;      // 外环环周期（电流环计算周期为定时器周期T，速度环为2T，位置环为3T）
  218.     int32_t actual_current = get_curr_val();    // 读取当前电流值
  219.  
  220.     if (actual_current > TARGET_CURRENT_MAX)
  221.     {
  222.         actual_current = TARGET_CURRENT_MAX;
  223.     }
  224.  
  225.     if (is_motor_en == 1)                  		// 电机在使能状态下才进行控制处理
  226.     {
  227.         static int32_t Capture_Count = 0;    	// 当前时刻总计数值
  228.         static int32_t Last_Count = 0;       	// 上一时刻总计数值
  229.         float cont_val = 0;                  	// 当前控制值
  230.  
  231.         /* 当前时刻总计数值 = 计数器值 + 计数溢出次数 * ENCODER_TIM_PERIOD  */
  232.         Capture_Count = __HAL_TIM_GET_COUNTER(&TIM_EncoderHandle) + (Encoder_Overflow_Count * 65535);
  233.  
  234.         /* 位置环计算 */
  235.         if (louter_ring_timer++ % 3 == 0)
  236.         {
  237.             cont_val = location_pid_realize(&pid_location, Capture_Count);    // 进行 PID 计算
  238.  
  239.             /* 目标速度上限处理 */
  240.             if (cont_val > TARGET_SPEED_MAX)
  241.             {
  242.                 cont_val = TARGET_SPEED_MAX;
  243.             }
  244.             else if (cont_val < -TARGET_SPEED_MAX)
  245.             {
  246.                 cont_val = -TARGET_SPEED_MAX;
  247.             }
  248.  
  249.             set_pid_target(&pid_speed, cont_val);    // 设定速度的目标值
  250.  
  251.             int32_t temp = cont_val;
  252.             set_computer_value(SEND_TARGET_CMD, CURVES_CH2, &temp, 1);     // 给通道 2 发送目标值
  253.         }
  254.  
  255.         /* 速度环计算 */
  256.         static int32_t actual_speed = 0;             // 实际测得速度
  257.  
  258.         if (louter_ring_timer % 2 == 0)
  259.         {
  260.             /* 转轴转速 = 单位时间内的计数值 / 编码器总分辨率 * 时间系数  */
  261.             actual_speed = ((float)(Capture_Count - Last_Count) / 16 * 4 / 30) / (GET_BASIC_TIM_PERIOD() * 2 / 1000.0 / 60.0);
  262.  
  263.             /* 记录当前总计数值，供下一时刻计算使用 */
  264.             Last_Count = Capture_Count;
  265.  
  266.             cont_val = speed_pid_realize(&pid_speed, actual_speed);    		// 进行 PID 计算
  267.  
  268.             if (cont_val > 0)    					// 判断电机方向
  269.             {
  270.                 set_motor_direction(MOTOR_FWD);
  271.             }
  272.             else
  273.             {
  274.                 cont_val = -cont_val;
  275.                 set_motor_direction(MOTOR_REV);
  276.             }
  277.  
  278.             cont_val = (cont_val > TARGET_CURRENT_MAX) ? TARGET_CURRENT_MAX : cont_val;    // 电流上限处理
  279.             set_pid_target(&pid_curr, cont_val);    						// 设定电流的目标值
  280.  
  281.             int32_t temp = cont_val;
  282.             set_computer_value(SEND_TARGET_CMD, CURVES_CH3, &temp, 1);     	// 给通道 3 发送目标值
  283.         }
  284.  
  285.         /* 电流环计算 */
  286.         cont_val = curr_pid_realize(&pid_curr, actual_current);    			// 进行 PID 计算
  287.  
  288.         if (cont_val < 0)
  289.         {
  290.             cont_val = 0;    	// 下限处理
  291.         }
  292.         else if (cont_val > 5500)
  293.         {
  294.             cont_val = 5500;	// 速度上限处理
  295.         }
  296.  
  297.         set_motor_speed(cont_val);                                                 // 设置 PWM 占空比
  298.  
  299.         set_computer_value(SEND_FACT_CMD, CURVES_CH1, &Capture_Count,  1);         // 给通道 1 发送实际值
  300.     }
  301. }

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/盐析白兔/article/detail/963185
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号