devbox
我家小花儿
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

学习PID—步进电机速度环控制实现_pid控制算法对机器人的速度和转角进行精确控制

作者：我家小花儿 | 2024-08-04 21:49:21

赞

踩

pid控制算法对机器人的速度和转角进行精确控制

参考链接：学习PID—步进电机速度环控制实现

原理概述

        步进电机是一种数字信号驱动的电机，其主要优点之一就是拥有很好的开环控制能力，控制系统不需要传感器和相应电路的反馈电机信息。

        在负载不超载和脉冲频率合适的情况下，步进电机接收到的脉冲数和转子的角位移就是严格成正比关系。

        虽然步进电机可以很好的开环控制，但实际在一些开环系统中，步进电机有可能由于自身性能及系统机械结构等因素的影响，在快速启停或负载突变时出现失步、过冲甚至堵转，控制器无法知晓和矫正，这些现象在某些对精度要求较高的系统中可能导致严重后果。

        而加入传感器反馈组成闭环系统后，可以检测是否有失步等现象发生并及时纠正偏差。

        步进电机闭环控制方案有很多种,可以完全控制步进电机的转矩和位置，改善步进电机的转矩频率特性，降低发热和平均功耗，提高电机运行效率。

        平时听到的一些控制名词，比如速度环、位置环和电流环这些，就可以用于步进电机的闭环控制，当然这些名词同样适用于其他的电机闭环系统。

        下面我们通过步进电机转速作为被控量（也就是速度环），使用旋转编码器作为反馈传感器，PID 算法进行控制的闭环控制系统。

图片

速度闭环控制–增量式 PID

        下面我们可以通过分别创建：

pid.h 和 pid.c 文件用来存放 PID 控制器相关程序。

stepper_ctrl.c、stepper_ctrl.h 文件用来存步进电机速度环控制程序及相关宏定义

操作步骤

•定时器 IO 配置

•步进电机、编码器相关外设初始化

•速度闭环控制实现

•PID 参数整定

测试环境

STM32F103VE版本以上,外部高速晶振：8MHz,RTC晶振：32.768KHz。

各总线运行时钟：系统时钟 = SYCCLK = AHB = 72MHz，APB2 = 72MHz ，APB1 = 36MHz

步进电机驱动器接口

如图所示：

图片

编码器与步进电机的接线

图片

代码解析

        第一步查看stepper_ctrl.h文件，通过这个文件是初始化步进电机GPIO等

  1.  
  2. #ifndef __STEP_MOTOR_INIT_H
  3. #define    __STEP_MOTOR_INIT_H
  4.  
  5. #include "stm32f1xx_hal.h"
  6.  
  7. //Motor 方向 
  8. #define MOTOR_DIR_PIN                      GPIO_PIN_6  
  9. #define MOTOR_DIR_GPIO_PORT                GPIOE               
  10. #define MOTOR_DIR_GPIO_CLK_ENABLE()       __HAL_RCC_GPIOE_CLK_ENABLE()
  11.  
  12. //Motor 使能 
  13. #define MOTOR_EN_PIN                      GPIO_PIN_5
  14. #define MOTOR_EN_GPIO_PORT                GPIOE                       
  15. #define MOTOR_EN_GPIO_CLK_ENABLE()        __HAL_RCC_GPIOE_CLK_ENABLE()
  16.  
  17. //Motor 脉冲
  18. #define MOTOR_PUL_IRQn                  TIM8_CC_IRQn
  19. #define MOTOR_PUL_IRQHandler            TIM8_CC_IRQHandler
  20.  
  21. #define MOTOR_PUL_TIM                   TIM8
  22. #define MOTOR_PUL_CLK_ENABLE()              __HAL_RCC_TIM8_CLK_ENABLE()
  23.  
  24. #define MOTOR_PUL_PORT                         GPIOC
  25. #define MOTOR_PUL_PIN                       GPIO_PIN_6
  26. #define MOTOR_PUL_GPIO_CLK_ENABLE()          __HAL_RCC_GPIOC_CLK_ENABLE()
  27. #define MOTOR_PUL_CHANNEL_x             TIM_CHANNEL_1
  28.  
  29. #define MOTOR_TIM_IT_CCx                TIM_IT_CC1
  30. #define MOTOR_TIM_FLAG_CCx              TIM_FLAG_CC1
  31.  
  32. /*频率相关参数*/
  33. //定时器实际时钟频率为：72MHz/TIM_PRESCALER
  34.  
  35. //具体需要的频率可以自己计算
  36. #define TIM_PRESCALER                16  /*补充：对F103例程测试，提高分频利于位置环稳定状态*/
  37. // 定义定时器周期，输出比较模式周期设置为0xFFFF
  38. #define TIM_PERIOD                   0xFFFF
  39.  
  40. /************************************************************/
  41. #define HIGH GPIO_PIN_SET       //高电平
  42. #define LOW  GPIO_PIN_RESET     //低电平
  43.  
  44. #define ON  LOW                 //开
  45. #define OFF HIGH                //关
  46.  
  47. #define CW  HIGH                //顺时针
  48. #define CCW LOW                 //逆时针
  49.  
  50. //控制使能引脚
  51. /* 带参宏，可以像内联函数一样使用 */
  52. #define MOTOR_EN(x)                    HAL_GPIO_WritePin(MOTOR_EN_GPIO_PORT,MOTOR_EN_PIN,x)
  53. #define MOTOR_PUL(x)                HAL_GPIO_WritePin(MOTOR_PUL_GPIO_PORT,MOTOR_PUL_PIN,x)
  54. #define MOTOR_DIR(x)                HAL_GPIO_WritePin(MOTOR_DIR_GPIO_PORT,MOTOR_DIR_PIN,x)
  55.  
  56. #define MOTOR_EN_TOGGLE     HAL_GPIO_TogglePin(MOTOR_EN_GPIO_PORT,MOTOR_EN_PIN)
  57. #define MOTOR_PUL_TOGGLE    HAL_GPIO_TogglePin(MOTOR_PUL_PORT,MOTOR_PUL_PIN)
  58.  
  59. extern TIM_HandleTypeDef TIM_StepperHandle;
  60. extern void stepper_Init(void);
  61. #endif /* __STEP_MOTOR_INIT_H */

stepper_ctrl.h 步进电机的步距角、驱动器细分数和 PID 控制用到的目标速度

        步进电机本身的参数和闭环控制需要用到的参数，包括步进电机的步距角、驱动器细分数和 PID 控制用到的目标速度等。

        其中宏 PULSE_RATIO 是细分后的步进电机单圈脉冲数与编码器单圈脉冲数的比值，因为在整个速度闭环控制系统中，反馈和 PID 计算得出的都是编码器的脉冲数。

  1.  
  2. #ifndef __STEP_MOTOR_CTRL_H
  3. #define    __STEP_MOTOR_CTRL_H
  4.  
  5. #include "stepper_init.h"
  6. #include "encoder.h"
  7.  
  8. /*宏定义*/
  9. /*******************************************************/
  10. #define T1_FREQ           (SystemCoreClock/TIM_PRESCALER) // 频率ft值
  11.  
  12. /*电机单圈参数*/
  13. #define STEP_ANGLE                1.8f                        //步进电机的步距角 单位：度
  14. #define FSPR              ((float)(360.0f/STEP_ANGLE))//步进电机的一圈所需脉冲数
  15.  
  16. #define MICRO_STEP        32                                  //细分器细分数 
  17. #define SPR               (FSPR*MICRO_STEP)           //细分后一圈所需脉冲数
  18.  
  19. #define PULSE_RATIO       ((float)(SPR/ENCODER_TOTAL_RESOLUTION))//步进电机单圈脉冲数与编码器单圈脉冲的比值
  20. #define TARGET_DISP       2                    //步进电机运动时的目标圈数，单位：转
  21. #define SPEED_LIMIT       10000                //最大启动速度限制
  22.  
  23. #define SAMPLING_PERIOD   50                          //PID采样频率，单位Hz
  24.  
  25. .......
  26. ........
  27. ........
  28.  
  29. void MSD_ENA(int NewState);
  30. void Set_Stepper_Stop(void);
  31. void Set_Stepper_Start(void);
  32. void Stepper_Speed_Ctrl(void);
  33.  
  34. #endif /* __STEP_MOTOR_CTRL_H */

        定义了一个结构体 __SYS_STATUS ，用来管理驱动器和电机的运行状态。

  1.  
  2. typedef struct {
  3.   unsigned char stepper_dir : 1;               //步进电机方向
  4.   unsigned char stepper_running : 1;           //步进电机运行状态
  5.   unsigned char MSD_ENA : 1;                   //驱动器使能状态
  6. }__SYS_STATUS;

增量式 PID

        PID 控制器的入口参数从原来的目标值更改为了反馈回来的实际值，而目标值在控制器外赋值，控制器的返回值变为 PID 计算得出的增量值，实际值的累加则放到了控制器外。

        整个增量式 PID 控制器的原理并没有变化，只是调整了部分代码的组织逻辑，这么做可以更方便的在程序的其他位置调用 PID 控制器。

如图所示：

图片

步进电机闭环控制系统

  1. /**
  2.   * @brief  步进电机刹车
  3.   * @param  无
  4.   * @retval 无
  5.   */
  6. void Set_Stepper_Stop(void)
  7. {
  8.   /*失能比较通道*/
  9.     TIM_CCxChannelCmd(MOTOR_PUL_TIM,MOTOR_PUL_CHANNEL_x,TIM_CCx_DISABLE);
  10.   sys_status.stepper_running = 0;
  11. }
  12.  
  13. /**
  14.   * @brief  启动步进电机
  15.   * @param  无
  16.   * @retval 无
  17.   */
  18. void Set_Stepper_Start(void)
  19. {
  20.   /*使能驱动器*/
  21.   MSD_ENA(0);
  22.   /*使能比较通道输出*/
  23.     TIM_CCxChannelCmd(MOTOR_PUL_TIM,MOTOR_PUL_CHANNEL_x,TIM_CCx_ENABLE);
  24.   sys_status.MSD_ENA = 1;
  25.   sys_status.stepper_running = 1;
  26. }
  27.  
  28. /**
  29.   * @brief  步进电机增量式PID控制
  30.   * @retval 无
  31.   * @note   基本定时器中断内调用
  32.   */
  33. void Stepper_Speed_Ctrl(void)
  34. {
  35.   /* 编码器相关变量 */
  36.   static __IO int32_t last_count = 0;
  37.   __IO int32_t capture_count = 0;
  38.   __IO int32_t capture_per_unit = 0;
  39.   /* 经过pid计算后的期望值 */
  40.   static __IO float cont_val = 0.0f;
  41.  
  42.   __IO float timer_delay = 0.0f;
  43.  
  44.   /* 当电机运动时才启动pid计算 */
  45.   if((sys_status.MSD_ENA == 1) && (sys_status.stepper_running == 1))
  46.   {
  47.     /* 计算单个采样时间内的编码器脉冲数 */
  48.     capture_count =(int)__HAL_TIM_GET_COUNTER(&TIM_EncoderHandle) + (encoder_overflow_count * ENCODER_TIM_PERIOD);
  49.  
  50.     /* 单位时间内的编码器脉冲数作为实际值传入pid控制器 */
  51.     cont_val += PID_realize((float)capture_count);// 进行 PID 计算
  52.  
  53.     /* 判断速度方向 */
  54.     cont_val > 0 ? (MOTOR_DIR(CW)) : (MOTOR_DIR(CCW));
  55.  
  56.     /* 计算得出的期望值取绝对值 */
  57.     timer_delay = fabsf(cont_val);
  58.  
  59.          /* 限制最大启动速度 */
  60.     timer_delay >= SPEED_LIMIT ? (timer_delay = SPEED_LIMIT) : timer_delay;
  61.  
  62.     /* 计算比较计数器的值 */
  63.      OC_Pulse_num = ((uint16_t)(T1_FREQ / ((float)timer_delay * PULSE_RATIO))) >> 1;
  64.      printf("实际值：%d，目标值：%.0f\r\n", capture_per_unit, pid.target_val);// 打印实际值和目标值 
  65.   }
  66.   else
  67.   {
  68.     /*停机状态所有参数清零*/
  69.     last_count = 0;
  70.     cont_val = 0;
  71.     pid.actual_val = 0;
  72.     pid.err = 0;
  73.     pid.err_last = 0;
  74.     pid.err_next = 0;
  75.   }

定义了一些用于编码器测速和 PID 计算的中间变量

        判断驱动器和电机运行状态，如果驱动器使能并且电机处于运动状态，才能执行闭环控制

  1.  
  2. if((sys_status.MSD_ENA == 1) && (sys_status.stepper_running == 1))

        读取编码器计数值并计算在单个采样周期中的计数值 capture_per_unit ，单位是脉冲每毫秒，实际表示编码器脉冲的频率，这里为了后续计算方便并没有写成以转每秒为单位的速度；

  1.  
  2.  capture_count =(int)__HAL_TIM_GET_COUNTER(&TIM_EncoderHandle) + (encoder_overflow_count * ENCODER_TIM_PERIOD);

        在电机停止或由运行变为停止时，需要清零编码器读数的中间值和 PID 控制器中的累加数据，以免影响电机再次启动时的控制效果。

  1.  
  2. /*停机状态所有参数清零*/
  3. last_count = 0;
  4. cont_val = 0;
  5. pid.actual_val = 0;
  6. pid.err = 0;
  7. pid.err_last = 0;
  8. pid.err_next = 0;

执行过程

        整个 Stepper_Speed_Ctrl 闭环控制函数中，传入 PID 和 PID 输出的参数都是编码器的数据，也就是编码器的脉冲频率，但是实际被控量是步进电机的转轴速度，需要做转换。

        将编码器的脉冲频率 capture_per_unit 乘上一个系数PULSE_RATIO 便可得到步进电机所需的脉冲频率，这个系数是由步进电机经过细分后转轴转一圈所需的脉冲数，与编码器转一圈发出的脉冲数之间的比值得出。

        不过此时的频率还是以 ms为单位的，为了后续计算方便，需要统一成以 s 为单位，因为本例程的采样周期是 20ms，所以单位转换只需要乘上 1s 内的采样次数 50 即可。

        得到步进电机需要的脉冲频率后，把它转换成可以写入捕获比较寄存器的值。

        当定时器配置为输出比较模式时，通过修改捕获比较寄存器当中的值，可以改变步进电机脉冲的周期，从而改变电机转速。

定时器控制

        基本定时器 TIM6 的定时中断中循环调用闭环控制程序，TIM6 配置为 20ms 中断一次，也就是说闭环控制的采样周期是 20ms。

  1.  
  2. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  3. {
  4.   /* 判断触发中断的定时器 */
  5.   if(htim->Instance == BASIC_TIM)
  6.   {
  7.     Stepper_Speed_Ctrl();
  8.   }
  9.   else if(htim->Instance == ENCODER_TIM)
  10.   {  
  11.     /* 判断当前计数方向 */
  12.     if(__HAL_TIM_IS_TIM_COUNTING_DOWN(htim))
  13.       /* 下溢 */
  14.       encoder_overflow_count--;
  15.     else
  16.       /* 上溢 */
  17.       encoder_overflow_count++;
  18.   }
  19. }

main

  1.  
  2. #include "main.h"
  3. #include <stdio.h>
  4. #include <stdlib.h>
  5. #include "usart.h"
  6. #include "stepper_init.h"
  7. #include "key.h"
  8. #include "led.h"
  9. #include "pid.h"
  10. #include "tim.h"
  11. #include "stepper_ctrl.h"
  12. #include "encoder.h"
  13. #include "protocol.h"
  14.  
  15. extern _pid pid;
  16. extern int pid_status;
  17.  
  18. /**
  19.   * @brief  主函数
  20.   * @param  无
  21.   * @retval 无
  22.   */
  23. int main(void) 
  24. {
  25.     /* 初始化系统时钟为72MHz */
  26.     SystemClock_Config();
  27.     /* 开启复用寄存器时钟 */
  28.     __HAL_RCC_SYSCFG_CLK_ENABLE();
  29.  
  30.     /*补充：PID例程中 MOTOR_PUL_IRQn 优先级需要调为最高 */    
  31.     /* Set Interrupt Group Priority */
  32.     HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);    
  33.  
  34.     /*初始化USART 配置模式为 115200 8-N-1，中断接收*/
  35.     DEBUG_USART_Config();
  36.     protocol_init();          /* 初始化串口通信协议 */
  37.     HAL_InitTick(5);
  38.     /*按键中断初始化*/
  39.     Key_GPIO_Config();    
  40.     /*led初始化*/
  41.     LED_GPIO_Config();
  42.     /* 初始化基本定时器定时，20ms产生一次中断 */
  43.     TIMx_Configuration();
  44.     /* 编码器接口初始化 */
  45.     Encoder_Init();
  46.     /*步进电机初始化*/
  47.     stepper_Init();
  48.     /* 上电默认停止电机 */
  49.     Set_Stepper_Stop();
  50.     /* PID算法参数初始化 */
  51.     PID_param_init();
  52.  
  53.     /* 目标速度转换为编码器的脉冲数作为pid目标值 */
  54.     pid.target_val = TARGET_DISP * ENCODER_TOTAL_RESOLUTION;
  55.     while(1)
  56.     {
  57.     /* 接收数据处理 */
  58.     receiving_process();
  59.  
  60.     /* 扫描KEY1，启动电机 */
  61.     if( Key_Scan(KEY1_GPIO_PORT,KEY1_PIN) == KEY_ON  )
  62.     {
  63.         Set_Stepper_Start();
  64.     }
  65.     /* 扫描KEY2，停止电机 */
  66.     if( Key_Scan(KEY2_GPIO_PORT,KEY2_PIN) == KEY_ON  )
  67.     {
  68.         Set_Stepper_Stop();     
  69.     }
  70.     /* 扫描KEY3，增大目标位置 */
  71.     if( Key_Scan(KEY3_GPIO_PORT,KEY3_PIN) == KEY_ON  )
  72.     {
  73.         /* 位置增加2圈 */
  74.         pid.target_val += 8000;
  75.     }
  76.     /* 扫描KEY4，减小目标位置 */
  77.     if( Key_Scan(KEY4_GPIO_PORT,KEY4_PIN) == KEY_ON  )
  78.     {
  79.         /* 位置减小2圈 */
  80.         pid.target_val -= 8000;
  81.     }
  82. }     
  83.  
  84. /**
  85.   * @brief  定时器更新事件回调函数
  86.   * @param  无
  87.   * @retval 无
  88.   */
  89. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  90. {
  91.   /* 判断触发中断的定时器 */
  92.   if(htim->Instance == BASIC_TIM)
  93.   {
  94.     Stepper_Speed_Ctrl();
  95.   }
  96.   else if(htim->Instance == ENCODER_TIM)
  97.   {  
  98.     /* 判断当前计数方向 */
  99.     if(__HAL_TIM_IS_TIM_COUNTING_DOWN(htim))
  100.       /* 下溢 */
  101.       encoder_overflow_count--;
  102.     else
  103.       /* 上溢 */
  104.       encoder_overflow_count++;
  105.   }
  106. }
  107.  
  108. /**
  109.   * @brief  System Clock Configuration
  110.   *         The system Clock is configured as follow : 
  111.   *            System Clock source            = PLL (HSE)
  112.   *            SYSCLK(Hz)                     = 72000000
  113.   *            HCLK(Hz)                       = 72000000
  114.   *            AHB Prescaler                  = 1
  115.   *            APB1 Prescaler                 = 2
  116.   *            APB2 Prescaler                 = 1
  117.   *            HSE Frequency(Hz)              = 8000000
  118.   *            HSE PREDIV1                    = 2
  119.   *            PLLMUL                         = 9
  120.   *            Flash Latency(WS)              = 0
  121.   * @param  None
  122.   * @retval None
  123.   */
  124. void SystemClock_Config(void)
  125. {
  126.   RCC_ClkInitTypeDef clkinitstruct = {0};
  127.   RCC_OscInitTypeDef oscinitstruct = {0};
  128.  
  129.   /* Enable HSE Oscillator and activate PLL with HSE as source */
  130.   oscinitstruct.OscillatorType  = RCC_OSCILLATORTYPE_HSE;
  131.   oscinitstruct.HSEState        = RCC_HSE_ON;
  132.   oscinitstruct.HSEPredivValue  = RCC_HSE_PREDIV_DIV1;
  133.   oscinitstruct.PLL.PLLState    = RCC_PLL_ON;
  134.   oscinitstruct.PLL.PLLSource   = RCC_PLLSOURCE_HSE;
  135.   oscinitstruct.PLL.PLLMUL      = RCC_PLL_MUL9;
  136.   if (HAL_RCC_OscConfig(&oscinitstruct)!= HAL_OK)
  137.   {
  138.     /* Initialization Error */
  139.     while(1); 
  140.   }
  141.  
  142.   /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */
  143.   clkinitstruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2);
  144.   clkinitstruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  145.   clkinitstruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  146.   clkinitstruct.APB2CLKDivider = RCC_HCLK_DIV1;
  147.   clkinitstruct.APB1CLKDivider = RCC_HCLK_DIV2;  
  148.   if (HAL_RCC_ClockConfig(&clkinitstruct, FLASH_LATENCY_2)!= HAL_OK)
  149.   {
  150.     while(1); 
  151.   }
  152. }

encoder.h

  1.  
  2. #ifndef __ENCOEDER_H
  3. #define    __ENCOEDER_H
  4.  
  5. #include "stm32f1xx.h"
  6.  
  7. /* 定时器选择 */
  8. #define ENCODER_TIM                            TIM4
  9. #define ENCODER_TIM_CLK_ENABLE()                       __HAL_RCC_TIM4_CLK_ENABLE()
  10. #define ENCODER_TIM_AF_CLK_ENABLE()                   __HAL_AFIO_REMAP_TIM4_ENABLE()
  11.  
  12. /* 定时器溢出值 */        
  13. #define ENCODER_TIM_PERIOD                     65535
  14. /* 定时器预分频值 */
  15. #define ENCODER_TIM_PRESCALER                  0      
  16.  
  17. /* 定时器中断 */
  18. #define ENCODER_TIM_IRQn                       TIM4_IRQn
  19. #define ENCODER_TIM_IRQHandler                 TIM4_IRQHandler
  20.  
  21. /* 编码器接口引脚 */
  22. #define ENCODER_TIM_CH1_GPIO_CLK_ENABLE()      __HAL_RCC_GPIOD_CLK_ENABLE()
  23. #define ENCODER_TIM_CH1_GPIO_PORT              GPIOD
  24. #define ENCODER_TIM_CH1_PIN                    GPIO_PIN_12
  25.  
  26. #define ENCODER_TIM_CH2_GPIO_CLK_ENABLE()      __HAL_RCC_GPIOD_CLK_ENABLE()
  27. #define ENCODER_TIM_CH2_GPIO_PORT              GPIOD
  28. #define ENCODER_TIM_CH2_PIN                    GPIO_PIN_13
  29.  
  30. /* 编码器接口倍频数 */
  31. #define ENCODER_MODE                           TIM_ENCODERMODE_TI12
  32.  
  33. /* 编码器接口输入捕获通道相位设置 */
  34. #define ENCODER_IC1_POLARITY                   TIM_ICPOLARITY_FALLING
  35. #define ENCODER_IC2_POLARITY                   TIM_ICPOLARITY_RISING
  36.  
  37. /* 编码器物理分辨率 */
  38. #define ENCODER_RESOLUTION                     1000
  39.  
  40. /* 经过倍频之后的总分辨率 */
  41. #if ((ENCODER_MODE == TIM_ENCODERMODE_TI1) || (ENCODER_MODE == TIM_ENCODERMODE_TI2))
  42.   #define ENCODER_TOTAL_RESOLUTION             (ENCODER_RESOLUTION * 2)  /* 2倍频后的总分辨率 */
  43. #else
  44.   #define ENCODER_TOTAL_RESOLUTION             (ENCODER_RESOLUTION * 4)  /* 4倍频后的总分辨率 */
  45. #endif
  46.  
  47.  
  48. extern __IO int16_t encoder_overflow_count;
  49. extern TIM_HandleTypeDef TIM_EncoderHandle;
  50.  
  51. void Encoder_Init(void);
  52.  
  53. #endif   /* __BSP_ENCODER_H */

encoder.c

  1.  
  2. #include "encoder.h"
  3.  
  4. /* 定时器溢出次数 */
  5. __IO int16_t encoder_overflow_count = 0;
  6. TIM_HandleTypeDef TIM_EncoderHandle;
  7.  
  8. /**
  9.   * @brief  编码器接口引脚初始化
  10.   * @param  无
  11.   * @retval 无
  12.   */
  13. static void Encoder_GPIO_Init(void)
  14. {
  15.   GPIO_InitTypeDef GPIO_InitStruct = {0};
  16.  
  17.   /* 定时器通道引脚端口时钟使能 */
  18.   ENCODER_TIM_CH1_GPIO_CLK_ENABLE();
  19.   ENCODER_TIM_CH2_GPIO_CLK_ENABLE();
  20.   /* 设置重映射 */
  21.   ENCODER_TIM_AF_CLK_ENABLE();
  22.  
  23.   /* 设置输入类型 */
  24.   GPIO_InitStruct.Mode = GPIO_MODE_AF_INPUT;
  25.   /* 设置上拉 */
  26.   GPIO_InitStruct.Pull = GPIO_PULLUP;
  27.   /* 设置引脚速率 */
  28.   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
  29.  
  30.   /* 选择要控制的GPIO引脚 */    
  31.   GPIO_InitStruct.Pin = ENCODER_TIM_CH1_PIN;
  32.  
  33.   /* 调用库函数，使用上面配置的GPIO_InitStructure初始化GPIO */
  34.   HAL_GPIO_Init(ENCODER_TIM_CH1_GPIO_PORT, &GPIO_InitStruct);
  35.  
  36.   /* 选择要控制的GPIO引脚 */    
  37.   GPIO_InitStruct.Pin = ENCODER_TIM_CH2_PIN;
  38.   /* 调用库函数，使用上面配置的GPIO_InitStructure初始化GPIO */
  39.   HAL_GPIO_Init(ENCODER_TIM_CH2_GPIO_PORT, &GPIO_InitStruct);
  40. }
  41.  
  42. /**
  43.   * @brief  配置TIMx编码器模式
  44.   * @param  无
  45.   * @retval 无
  46.   */
  47. static void TIM_Encoder_Init(void)
  48. { 
  49.   TIM_Encoder_InitTypeDef Encoder_ConfigStructure;
  50.  
  51.   /* 使能编码器接口时钟 */
  52.   ENCODER_TIM_CLK_ENABLE();
  53.  
  54.   /* 定时器初始化设置 */
  55.   TIM_EncoderHandle.Instance = ENCODER_TIM;
  56.   TIM_EncoderHandle.Init.Prescaler = ENCODER_TIM_PRESCALER;
  57.   TIM_EncoderHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
  58.   TIM_EncoderHandle.Init.Period = ENCODER_TIM_PERIOD;
  59.   TIM_EncoderHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  60.   TIM_EncoderHandle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  61.  
  62.   /* 设置编码器倍频数 */
  63.   Encoder_ConfigStructure.EncoderMode = ENCODER_MODE;
  64.   /* 编码器接口通道1设置 */
  65.   Encoder_ConfigStructure.IC1Polarity = ENCODER_IC1_POLARITY;
  66.   Encoder_ConfigStructure.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  67.   Encoder_ConfigStructure.IC1Prescaler = TIM_ICPSC_DIV1;
  68.   Encoder_ConfigStructure.IC1Filter = 0;
  69.   /* 编码器接口通道2设置 */
  70.   Encoder_ConfigStructure.IC2Polarity = ENCODER_IC2_POLARITY;
  71.   Encoder_ConfigStructure.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  72.   Encoder_ConfigStructure.IC2Prescaler = TIM_ICPSC_DIV1;
  73.   Encoder_ConfigStructure.IC2Filter = 0;
  74.   /* 初始化编码器接口 */
  75.   HAL_TIM_Encoder_Init(&TIM_EncoderHandle, &Encoder_ConfigStructure);
  76.  
  77.   /* 清零计数器 */
  78.   __HAL_TIM_SET_COUNTER(&TIM_EncoderHandle, 0);
  79.  
  80.   /* 清零中断标志位 */
  81.   __HAL_TIM_CLEAR_IT(&TIM_EncoderHandle,TIM_IT_UPDATE);
  82.   /* 使能定时器的更新事件中断 */
  83.   __HAL_TIM_ENABLE_IT(&TIM_EncoderHandle,TIM_IT_UPDATE);
  84.   /* 设置更新事件请求源为：计数器溢出 */
  85.   __HAL_TIM_URS_ENABLE(&TIM_EncoderHandle);
  86.  
  87.   /* 设置中断优先级 */
  88.   HAL_NVIC_SetPriority(ENCODER_TIM_IRQn, 5, 1);
  89.   /* 使能定时器中断 */
  90.   HAL_NVIC_EnableIRQ(ENCODER_TIM_IRQn);
  91.  
  92.   /* 使能编码器接口 */
  93.   HAL_TIM_Encoder_Start(&TIM_EncoderHandle, TIM_CHANNEL_ALL);
  94. }
  95.  
  96. /**
  97.   * @brief  编码器接口初始化
  98.   * @param  无
  99.   * @retval 无
  100.   */
  101. void Encoder_Init(void)
  102. {
  103.   Encoder_GPIO_Init();    /* 引脚初始化 */
  104.   TIM_Encoder_Init();     /* 配置编码器接口 */
  105. }

pid.c

  1.  
  2. /* Includes ------------------------------------------------------------------*/
  3. #include "pid.h"
  4. #include "math.h"
  5. #include "stepper_ctrl.h"
  6. #include "protocol.h"
  7.  
  8. /* 定义全局变量 */
  9. _pid pid;
  10. float set_point=0.0;
  11. int pid_status=0;
  12.  
  13. /**
  14.   * @brief  PID参数初始化
  15.   *    @note     无
  16.   * @retval 无
  17.   */
  18. void PID_param_init()
  19. {
  20.     /* 初始化参数 */
  21.     pid.target_val=0.0;                
  22.     pid.actual_val=0.0;
  23.     pid.err = 0.0;
  24.     pid.err_last = 0.0;
  25.     pid.err_next = 0.0;
  26.     pid.Kp = 1.2;
  27.     pid.Ki = 0;
  28.     pid.Kd = 0;
  29. }
  30.  
  31. /**
  32.   * @brief  设置目标值
  33.   * @param  val        目标值
  34.   *    @note     无
  35.   * @retval 无
  36.   */
  37. void set_pid_actual(float temp_val)
  38. {
  39.   pid.target_val = temp_val;    // 设置当前的目标值
  40. }
  41.  
  42. /**
  43.   * @brief  获取目标值
  44.   * @param  无
  45.     *    @note     无
  46.   * @retval 目标值
  47.   */
  48. float get_pid_actual(void)
  49. {
  50.   return pid.target_val;    // 设置当前的目标值
  51. }
  52.  
  53. /**
  54.   * @brief  设置比例、积分、微分系数
  55.   * @param  p：比例系数 P
  56.   * @param  i：积分系数 i
  57.   * @param  d：微分系数 d
  58.     *    @note     无
  59.   * @retval 无
  60.   */
  61. void set_p_i_d(float p, float i, float d)
  62. {
  63.     pid.Kp = p;    // 设置比例系数 P
  64.     pid.Ki = i;    // 设置积分系数 I
  65.     pid.Kd = d;    // 设置微分系数 D
  66. }
  67.  
  68. /**
  69.   * @brief  增量式PID算法实现
  70.   * @param  val：当前实际值
  71.     *    @note     无
  72.   * @retval 通过PID计算后的输出
  73.   */
  74. float PID_realize(float temp_val) 
  75. {
  76.     /*传入实际值*/
  77.     pid.actual_val = temp_val;
  78.     /*计算目标值与实际值的误差*/
  79.     pid.err=pid.target_val-pid.actual_val;
  80.  
  81.     /*PID算法实现*/
  82.     float increment_val = pid.Kp*(pid.err - pid.err_next) + pid.Ki*pid.err + pid.Kd*(pid.err - 2 * pid.err_next + pid.err_last);
  83.     /*传递误差*/
  84.     pid.err_last = pid.err_next;
  85.     pid.err_next = pid.err;
  86.     /*返回增量值*/
  87.     return increment_val;
  88. }

pid.h

  1. #ifndef __PID_H
  2. #define    __PID_H
  3. #include "stm32f1xx.h"
  4. #include "usart.h"
  5. #include <stdio.h>
  6. #include <stdlib.h>
  7.  
  8. /*pid*/
  9. typedef struct
  10. {
  11.   float target_val;     //目标值
  12.     float actual_val;     //实际值
  13.     float err;            //定义当前偏差值
  14.     float err_next;       //定义下一个偏差值
  15.     float err_last;       //定义上一个偏差值
  16.     float Kp, Ki, Kd;     //定义比例、积分、微分系数
  17. }_pid;
  18.  
  19.  
  20. extern void PID_param_init(void);
  21. extern void set_pid_actual(float temp_val);
  22. extern float get_pid_actual(void);
  23. extern void set_p_i_d(float p, float i, float d);
  24. extern float PID_realize(float temp_val);
  25. extern void time_period_fun(void);
  26.  
  27. #endif

位置式 PID

        创建了 4 个文件：pid.h 和 pid.c 文件用来存放 PID 控制器相关程序，stepper_ctrl.c、stepper_ctrl.h 文件用来存步进电机速度环控制程序及相关宏定义。

        stepper_init.h 和stepper_ctrl.h 中的宏定义与增量式 PID中的宏定义完全相同。

pid.c

        PID 控制器的入口参数从原来的目标值更改为了反馈回来的实际值，而目标值在控制器外赋值，控制器的返回值变为 PID 计算得出的位置值。

        整个位置式 PID 控制器的原理并没有变化，只是调整了部分代码的组织逻辑，这么做可以更方便的在程序的其他位置调用 PID 控制器。

  1.  
  2. #include "pid.h"
  3. #include "math.h"
  4. #include "stepper_ctrl.h"
  5. #include "protocol.h"
  6. /* 定义全局变量 */
  7. _pid pid;
  8. float set_point=0.0;
  9. int pid_status=0;
  10.  
  11. /**
  12.   * @brief  PID参数初始化
  13.     *    @note     无
  14.   * @retval 无
  15.   */
  16. void PID_param_init()
  17. {
  18.     /* 初始化参数 */
  19.     pid.target_val=0.0;                
  20.     pid.actual_val=0.0;
  21.     pid.err=0.0;
  22.     pid.err_last=0.0;
  23.     pid.integral=0.0;
  24.     pid.Kp = 1.2;
  25.     pid.Ki = 0;
  26.     pid.Kd = 0;
  27. }
  28.  
  29. /**
  30.   * @brief  设置目标值
  31.   * @param  val        目标值
  32.   *    @note     无
  33.   * @retval 无
  34.   */
  35. void set_pid_actual(float temp_val)
  36. {
  37.     pid.target_val = temp_val;    // 设置当前的目标值
  38. }
  39.  
  40. /**
  41.   * @brief  获取目标值
  42.   * @param  无
  43.   *    @note     无
  44.   * @retval 目标值
  45.   */
  46. float get_pid_actual(void)
  47. {
  48.   return pid.target_val;    // 设置当前的目标值
  49. }
  50.  
  51. /**
  52.   * @brief  设置比例、积分、微分系数
  53.   * @param  p：比例系数 P
  54.   * @param  i：积分系数 i
  55.   * @param  d：微分系数 d
  56.   *    @note     无
  57.   * @retval 无
  58.   */
  59. void set_p_i_d(float p, float i, float d)
  60. {
  61.     pid.Kp = p;    // 设置比例系数 P
  62.     pid.Ki = i;    // 设置积分系数 I
  63.     pid.Kd = d;    // 设置微分系数 D
  64. }
  65.  
  66. /**
  67.   * @brief  位置式PID算法实现
  68.   * @param  actual_val：当前实际值
  69.   *    @note     无
  70.   * @retval 通过PID计算后的输出
  71.   */
  72. float PID_realize(float actual_val) 
  73. {
  74.     /*传入实际值*/
  75.     pid.actual_val = actual_val;
  76.     /*计算目标值与实际值的误差*/
  77.     pid.err = pid.target_val - pid.actual_val;
  78.  
  79.     /*误差累积*/
  80.     pid.integral += pid.err;
  81.     /*PID算法实现*/
  82.     pid.actual_val = pid.Kp*pid.err
  83.                 + pid.Ki*pid.integral
  84.                 + pid.Kd*(pid.err-pid.err_last);
  85.     /*误差传递*/
  86.     pid.err_last = pid.err;
  87.     /*PID算法实现，并返回计算值*/
  88.     return pid.actual_val;
  89. }

步进电机闭环控制系统

  1.  
  2. #include "math.h"
  3. #include "tim.h"
  4. #include "stepper_ctrl.h"
  5. #include "pid.h"
  6. #include "protocol.h"
  7. extern _pid pid;
  8. extern __IO uint16_t OC_Pulse_num;     //比较输出的计数值
  9.  
  10. __SYS_STATUS sys_status = {0};
  11.  
  12. /**
  13.   * @brief  驱动器紧急停止
  14.   * @param  NewState：使能或者禁止
  15.   * @retval 无
  16.   */
  17. void MSD_ENA(int NewState)
  18. {
  19.     if(NewState)
  20.     {
  21.       //ENA失能，禁止驱动器输出，（脱机状态）此时电机为无保持力矩状态，可以手动旋转电机
  22.       MOTOR_EN(OFF);
  23.       sys_status.MSD_ENA = 0;            
  24.  
  25.     }
  26.     else
  27.     {
  28.       //ENA使能，此时电机为保持力矩状态
  29.       MOTOR_EN(ON);
  30.       sys_status.MSD_ENA = 1;            
  31.     }
  32. }
  33.  
  34. /**
  35.   * @brief  步进电机刹车
  36.   * @param  无
  37.   * @retval 无
  38.   */
  39. void Set_Stepper_Stop(void)
  40. {
  41.   /*失能比较通道*/
  42.     TIM_CCxChannelCmd(MOTOR_PUL_TIM,MOTOR_PUL_CHANNEL_x,TIM_CCx_DISABLE);
  43.     sys_status.stepper_running = 0;
  44. }
  45.  
  46. /**
  47.   * @brief  启动步进电机
  48.   * @param  无
  49.   * @retval 无
  50.   */
  51. void Set_Stepper_Start(void)
  52. {
  53.   /*使能驱动器*/
  54.   MSD_ENA(0);
  55.   /*使能比较通道输出*/
  56.     TIM_CCxChannelCmd(MOTOR_PUL_TIM,MOTOR_PUL_CHANNEL_x,TIM_CCx_ENABLE);
  57.   sys_status.MSD_ENA = 1;
  58.   sys_status.stepper_running = 1;
  59. }
  60.  
  61. /**
  62.   * @brief  步进电机位置式PID控制
  63.   * @retval 无
  64.   * @note   基本定时器中断内调用
  65.   */
  66. void Stepper_Speed_Ctrl(void)
  67. {
  68.   /* 编码器相关变量 */
  69.   __IO int32_t capture_per_unit = 0;
  70.   __IO int32_t capture_count = 0;
  71.   static __IO int32_t last_count = 0;
  72.   /* 经过pid计算后的期望值 */
  73.   __IO int32_t cont_val = 0;
  74.  
  75.   /* 当电机运动时才启动pid计算 */
  76.   if((sys_status.MSD_ENA == 1) && (sys_status.stepper_running == 1))
  77.   {
  78.     /* 计算单个采样时间内的编码器脉冲数 */
  79.     capture_count =(int)__HAL_TIM_GET_COUNTER(&TIM_EncoderHandle) + (encoder_overflow_count * ENCODER_TIM_PERIOD);
  80.  
  81.     /* 单位时间内的编码器脉冲数作为实际值传入pid控制器 */
  82.     cont_val = PID_realize((float)capture_count);// 进行 PID 计算
  83.  
  84.     /* 判断方向 */
  85.     cont_val > 0 ? (MOTOR_DIR(CW)) : (MOTOR_DIR(CCW));
  86.  
  87.     /* 对计算得出的期望值取绝对值 */
  88.     cont_val = abs(cont_val);
  89.  
  90.     /* 限制最大启动速度 */
  91.     cont_val >= SPEED_LIMIT ? (cont_val = SPEED_LIMIT) : cont_val;
  92.  
  93.     /* 计算比较计数器的值 */
  94.     OC_Pulse_num = ((uint16_t)(T1_FREQ / ((float)cont_val * PULSE_RATIO))) >> 1;
  95.     printf("实际值：%d，目标值：%.0f\r\n", capture_count, pid.target_val);// 打印实际值和目标值
  96.   }
  97.   else
  98.   {
  99.     capture_per_unit = 0;
  100.     cont_val = 0;
  101.     pid.actual_val = 0;
  102.     pid.err = 0;
  103.     pid.err_last = 0;
  104.     pid.integral = 0;
  105.   }
  106. }

main.c

  1.  
  2. /**
  3.   ******************************************************************************
  4.   * @file    main.c
  5.   * @author  fire
  6.   * @version V1.0
  7.   * @date    2020-xx-xx
  8.   * @brief   步进电机-位置环
  9.   ******************************************************************************
  10.   * @attention
  11.   *
  12.   * 实验平台:野火 F103-拂晓 STM32 开发板 
  13.   * 论坛    :http://www.firebbs.cn
  14.   * 淘宝    :https://fire-stm32.taobao.com
  15.   *
  16.   ******************************************************************************
  17.   */ 
  18.  
  19. /* Includes ------------------------------------------------------------------*/
  20. #include "main.h"
  21. #include <stdio.h>
  22. #include <stdlib.h>
  23. #include "./usart/bsp_debug_usart.h"
  24. #include "./stepper/bsp_stepper_init.h"
  25. #include "./key/bsp_key.h"
  26. #include "./led/bsp_led.h"
  27. #include "./pid/bsp_pid.h"
  28. #include "./tim/bsp_basic_tim.h"
  29. #include "./stepper/bsp_stepper_ctrl.h"
  30. #include "./Encoder/bsp_encoder.h"
  31. #include "./protocol/protocol.h"
  32.  
  33. extern _pid pid;
  34. extern int pid_status;
  35.  
  36. /**
  37.   * @brief  主函数
  38.   * @param  无
  39.   * @retval 无
  40.   */
  41. int main(void) 
  42. {
  43.     /* 初始化系统时钟为72MHz */
  44.     SystemClock_Config();
  45.     /* 开启复用寄存器时钟 */
  46.     __HAL_RCC_SYSCFG_CLK_ENABLE();
  47.     /* Set Interrupt Group Priority */
  48.     HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);    
  49.  
  50.     /*初始化USART 配置模式为 115200 8-N-1，中断接收*/
  51.     DEBUG_USART_Config();
  52.     protocol_init();          /* 初始化串口通信协议 */
  53.     HAL_InitTick(5);
  54.     /*按键中断初始化*/
  55.     Key_GPIO_Config();    
  56.     /*led初始化*/
  57.     LED_GPIO_Config();
  58.     /* 初始化基本定时器定时，20ms产生一次中断 */
  59.     TIMx_Configuration();
  60.     /* 编码器接口初始化 */
  61.     Encoder_Init();
  62.     /*步进电机初始化*/
  63.     stepper_Init();
  64.     /* 上电默认停止电机 */
  65.     Set_Stepper_Stop();
  66.     /* PID算法参数初始化 */
  67.     PID_param_init();
  68.  
  69.     /* 目标速度转换为编码器的脉冲数作为pid目标值 */
  70.     pid.target_val = TARGET_DISP * ENCODER_TOTAL_RESOLUTION;
  71.     while(1)
  72.     {
  73.         /* 接收数据处理 */
  74.         receiving_process();
  75.  
  76.         /* 扫描KEY1，启动电机 */
  77.         if( Key_Scan(KEY1_GPIO_PORT,KEY1_PIN) == KEY_ON  )
  78.         {
  79.             Set_Stepper_Start();
  80.         }
  81.         /* 扫描KEY2，停止电机 */
  82.         if( Key_Scan(KEY2_GPIO_PORT,KEY2_PIN) == KEY_ON  )
  83.         {
  84.             Set_Stepper_Stop();     
  85.         }
  86.         /* 扫描KEY3，增大目标位置 */
  87.         if( Key_Scan(KEY3_GPIO_PORT,KEY3_PIN) == KEY_ON  )
  88.         {
  89.             /* 位置增加2圈 */
  90.             pid.target_val += 8000;
  91.         }
  92.         /* 扫描KEY4，减小目标位置 */
  93.         if( Key_Scan(KEY4_GPIO_PORT,KEY4_PIN) == KEY_ON  )
  94.         {
  95.             /* 位置减小2圈 */
  96.             pid.target_val -= 8000;
  97.         }
  98.     }
  99. }     
  100.  
  101.  
  102. /**
  103.   * @brief  定时器更新事件回调函数
  104.   * @param  无
  105.   * @retval 无
  106.   */
  107. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  108. {
  109.   /* 判断触发中断的定时器 */
  110.   if(htim->Instance == BASIC_TIM)
  111.   {
  112.     Stepper_Speed_Ctrl();
  113.   }
  114.   else if(htim->Instance == ENCODER_TIM)
  115.   {  
  116.     /* 判断当前计数方向 */
  117.     if(__HAL_TIM_IS_TIM_COUNTING_DOWN(htim))
  118.       /* 下溢 */
  119.       encoder_overflow_count--;
  120.     else
  121.       /* 上溢 */
  122.       encoder_overflow_count++;
  123.   }
  124. }
  125.  
  126. void SystemClock_Config(void)
  127. {
  128.   RCC_ClkInitTypeDef clkinitstruct = {0};
  129.   RCC_OscInitTypeDef oscinitstruct = {0};
  130.  
  131.   /* Enable HSE Oscillator and activate PLL with HSE as source */
  132.   oscinitstruct.OscillatorType  = RCC_OSCILLATORTYPE_HSE;
  133.   oscinitstruct.HSEState        = RCC_HSE_ON;
  134.   oscinitstruct.HSEPredivValue  = RCC_HSE_PREDIV_DIV1;
  135.   oscinitstruct.PLL.PLLState    = RCC_PLL_ON;
  136.   oscinitstruct.PLL.PLLSource   = RCC_PLLSOURCE_HSE;
  137.   oscinitstruct.PLL.PLLMUL      = RCC_PLL_MUL9;
  138.   if (HAL_RCC_OscConfig(&oscinitstruct)!= HAL_OK)
  139.   {
  140.     /* Initialization Error */
  141.     while(1); 
  142.   }
  143.  
  144.   /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 
  145.      clocks dividers */
  146.   clkinitstruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2);
  147.   clkinitstruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  148.   clkinitstruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  149.   clkinitstruct.APB2CLKDivider = RCC_HCLK_DIV1;
  150.   clkinitstruct.APB1CLKDivider = RCC_HCLK_DIV2;  
  151.   if (HAL_RCC_ClockConfig(&clkinitstruct, FLASH_LATENCY_2)!= HAL_OK)
  152.   {
  153.     /* Initialization Error */
  154.     while(1); 
  155.   }
  156. }

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

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

  

闽ICP备14008679号