21ic电子技术开发论坛 › 单片机与嵌入式系统 › ST MCU › STM32实现编码器电机【速度与位置环闭环控制】 ...
返回列表 发新帖我要提问

[STM32F1] STM32实现编码器电机【速度与位置环闭环控制】

[复制链接]
 楼主| yutingwei 发表于 2023-7-29 16:59 | 显示全部楼层 |阅读模式
ST, STM, STM32, 位置环, 控制
一、硬件及接线说明
1.1 硬件平台
控制芯片：STM32F103ZET6

电机驱动：TB6612

电机类型：520编码器电机（12V 110RPM 减速比90）

1.2 接线说明
PWMA —— PE9（TIM1通道1）
STBY —— PF0
AIN1 —— PF1
AIN2 —— PF2
编码器A相 —— PA1（TIM2编码器模式）
编码器B相 —— PA0（TIM2编码器模式）
TIM6：产生1ms定时器中断（无需接线）

回复 赏

举报

相关帖子
 楼主| yutingwei 发表于 2023-7-29 17:01 | 显示全部楼层
二、CUBEMX配置
2.1 新建工程，配置时钟频率为72MHz
5782164c4d5619d86a.png
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:02 | 显示全部楼层
2.2 配置RCC，使用外部高速晶振
7349564c4d58bf13cf.png
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:02 | 显示全部楼层
2.3 Debug配置为Serial Wire模式
8273764c4d5a2f29c2.png
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:02 | 显示全部楼层
2.4 配置GPIO，PF0默认上拉
7849964c4d5af84fd1.png
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:03 | 显示全部楼层
2.5 配置定时器
1989264c4d5e875d81.png
2884064c4d5ede9207.png

1023364c4d5f3df477.png
配置完成后生成代码
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:04 | 显示全部楼层
三、代码实现
encoder.c
复制
  1. /* 此文件为编码器电机闭环调试，包括速度环和位置环
    

  2.  * 配置：TIM2(PA0、PA1)：编码器模式
    

  3.  *                          TIM1-CH1(PE9)：PWM输出
    

  4.  *                         IN1：PF1
    

  5.  *                          IN2: PF2
    

  6.  *              ENABLE: PF0
    

  7.  * 使用方法：1、初始化Motor_Init()
    

  8.  *                                          2、发送电流SetCurrent()
    

  9.  */
    

  10. #include  "encoder.h"
    


  11. 

  12. encoderMotor_t encoderMoto[ENCODER_MOTO_COUNT];                //编码器电机结构体
    


  13. 

  14. pid_t Encoder_Motor_Pid_Pos[ENCODER_MOTO_COUNT];  //编码器电机位置环PID结构体
    

  15. pid_t Encoder_Motor_Pid_Spd[ENCODER_MOTO_COUNT];  //编码器电机速度环PID结构体
    


  16. 


  17. 

  18. void Motor_Init(void)
    

  19. {
    

  20.                 HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);      //开启编码器定时器
    

  21.                 __HAL_TIM_ENABLE_IT(&htim2,TIM_IT_UPDATE);                  //开启编码器定时器更新中断,防溢出处理
    

  22.                 __HAL_TIM_SET_COUNTER(&htim2, 10000);                //编码器定时器初始值设定为10000
    

  23.         
    

  24.                 encoderMoto[0].IOControl.htim_pwm = htim1;
    

  25.                 encoderMoto[0].IOControl.IN1_Port = GPIOF;
    

  26.                 encoderMoto[0].IOControl.IN2_Port = GPIOF;
    

  27.                 encoderMoto[0].IOControl.IN1_Pin = GPIO_PIN_1;
    

  28.                 encoderMoto[0].IOControl.IN2_Pin = GPIO_PIN_2;
    

  29. }
    


  30. 


  31. 

  32. //
    

  33. /* 编码器电机发送电流函数
    

  34.  * motor：编码器电机参数结构体
    

  35.  * val：转动的速度或角度，SPEED最大为110，POSITION一圈为3960
    

  36.  * mode：模式选择：速度环：SPEED
    

  37.  *                                                                  位置环：POSITION
    

  38.  */
    

  39. void SetCurrent(encoderMotor_t *motor, int32_t val, uint32_t mode)
    

  40. {
    

  41.                 float pos_output,spd_output;
    

  42.                 if(mode == 1)
    

  43.                         spd_output = pid_calc(&Encoder_Motor_Pid_Spd[0], motor->speed, val);
    

  44.                 else
    

  45.                 {
    

  46.                         pos_output = pid_calc(&Encoder_Motor_Pid_Pos[0], motor->totalAngle, val);
    

  47.                         spd_output = pid_calc(&Encoder_Motor_Pid_Spd[0], motor->speed, pos_output);
    

  48.                 }
    


  49. 

  50.                 if(spd_output > 0)
    

  51.                 {
    

  52.                                 HAL_GPIO_WritePin(motor->IOControl.IN1_Port, motor->IOControl.IN1_Pin, GPIO_PIN_SET);                //控制正反转
    

  53.                                 HAL_GPIO_WritePin(motor->IOControl.IN2_Port, motor->IOControl.IN2_Pin, GPIO_PIN_RESET);
    

  54.                                 __HAL_TIM_SET_COMPARE(&motor->IOControl.htim_pwm, TIM_CHANNEL_1, (uint32_t)(spd_output));
    

  55.                 }
    

  56.                 else
    

  57.                 {
    

  58.                                 HAL_GPIO_WritePin(motor->IOControl.IN1_Port, motor->IOControl.IN1_Pin, GPIO_PIN_RESET);
    

  59.                                 HAL_GPIO_WritePin(motor->IOControl.IN2_Port, motor->IOControl.IN2_Pin, GPIO_PIN_SET);
    

  60.                                 __HAL_TIM_SET_COMPARE(&motor->IOControl.htim_pwm, TIM_CHANNEL_1, (uint32_t)(-spd_output));
    

  61.                 }
    

  62.         
    

  63. }
    


  64. 

  65. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
    

  66. {
    

  67.         static int16_t count = 0;
    

  68.         if(htim->Instance==htim6.Instance)                         //1ms中断
    

  69.         {
    

  70.                 count++;
    

  71.                 if(count >= 10)
    

  72.                 {
    

  73.                         count = 0;
    

  74.                         int16_t pluse = COUNTERNUM - RELOADVALUE/2;                                                                                        
    

  75.                         encoderMoto[0].totalAngle = pluse + encoderMoto[0].loopNum * RELOADVALUE/2;  
    

  76.                         encoderMoto[0].speed = (float)(encoderMoto[0].totalAngle - encoderMoto[0].lastAngle)/(4*PLUSE_OF_CIRCLE*RR)*6000;                        //进行速度计算,根据前文所说的,4倍频,编码器13位,减速比30,再乘以6000即为每分钟输出轴多少转
    

  77.                         encoderMoto[0].lastAngle = encoderMoto[0].totalAngle;         //更新转过的圈数
    

  78.                 }
    

  79.         }
    

  80.         else if(htim->Instance == htim2.Instance)      //如果是编码器更新中断,即10ms内,脉冲数超过了计数范围,需要进行处理
    

  81.         {
    

  82.                 if(COUNTERNUM < 10000)        encoderMoto[0].loopNum++;
    

  83.                 else if(COUNTERNUM > 10000)        encoderMoto[0].loopNum--;
    

  84.                 __HAL_TIM_SetCounter(&htim2, 10000);             //重新设定初始值                        
    

  85.         }
    

  86. }
    


  87.
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:04 | 显示全部楼层
encoder.h
复制
  1. #ifndef __ENCODER_H
    

  2. #define __ENCODER_H
    


  3. 

  4. #include "tim.h"
    

  5. #include "gpio.h"
    

  6. #include "main.h"
    

  7. #include "stm32_hal_legacy.h"
    

  8. #include "pid.h"
    


  9. 

  10. #define ENCODER_MOTO_COUNT                1                //编码器电机数量
    

  11. #define RR        90        //电机减速比
    

  12. #define PLUSE_OF_CIRCLE                11
    

  13. #define RELOADVALUE         __HAL_TIM_GetAutoreload(&htim2)    //获取自动装载值,本例中为20000
    

  14. #define COUNTERNUM                 __HAL_TIM_GetCounter(&htim2)        //获取编码器定时器中的计数值
    


  15. 


  16. 

  17. #define MOTOR_1                1
    


  18. 

  19. enum{
    

  20.     POSITION        = 0,
    

  21.     SPEED         = 1,
    

  22. };
    


  23. 

  24. /* 编码器电机接口定义结构体 */
    

  25. typedef struct _IOControl
    

  26. {
    

  27.         TIM_HandleTypeDef htim_encoder;
    

  28.         TIM_HandleTypeDef htim_pwm;
    

  29.         GPIO_TypeDef *IN1_Port;
    

  30.         GPIO_TypeDef *IN2_Port;
    

  31.         uint16_t IN1_Pin;
    

  32.         uint16_t IN2_Pin;
    

  33. }IOControl_t;
    


  34. 

  35. /* 编码器电机参数结构体 */
    

  36. typedef struct _EncoderMotor{
    

  37.         int8_t ID;
    

  38.         int16_t loopNum;          //防超上限
    

  39.         int32_t lastAngle;        //上1ms转的角度
    

  40.         int32_t totalAngle;       //总角度
    

  41.         float speed;              //电机输出轴转速，单位RPM
    

  42.         float set;
    

  43.         IOControl_t IOControl;
    

  44. }encoderMotor_t;
    


  45. 

  46. extern encoderMotor_t encoderMoto[ENCODER_MOTO_COUNT];
    

  47. extern pid_t Encoder_Motor_Pid_Pos[ENCODER_MOTO_COUNT];  //编码器电机位置环PID结构体
    

  48. extern pid_t Encoder_Motor_Pid_Spd[ENCODER_MOTO_COUNT];  //编码器电机速度环PID结构体
    


  49. 

  50. void SetCurrent(encoderMotor_t *motor, int32_t val, uint32_t mode);
    

  51. void Motor_Init(void);
    

  52. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim);
    


  53. 

  54. #endif
    


  55.
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:04 | 显示全部楼层
pid.c
复制
  1. #include "pid.h"
    


  2. 


  3. 

  4. void abs_limit(float *a, float ABS_MAX)
    

  5. {
    

  6.     if(*a > ABS_MAX)
    

  7.         *a = ABS_MAX;
    

  8.     if(*a < -ABS_MAX)
    

  9.         *a = -ABS_MAX;
    

  10. }
    


  11. 

  12. void PID_struct_init(pid_t* pid,
    

  13.                                                                                  uint32_t maxout,
    

  14.                                                                                  uint32_t intergral_limit,
    

  15.                                                                                  float         kp, 
    

  16.                                                                                  float         ki, 
    

  17.                                                                            float         kd)
    

  18. {
    

  19.     pid->IntegralLimit = intergral_limit;
    

  20.     pid->MaxOutput = maxout;
    

  21.     
    

  22.     pid->p = kp;
    

  23.     pid->i = ki;
    

  24.     pid->d = kd;
    

  25. }
    


  26. 


  27. 

  28. float pid_calc(pid_t* pid, float get, float set)
    

  29. {
    

  30.     pid->get = get;
    

  31.     pid->set = set;
    

  32.     pid->err = set - get;        /*set - measure，得到偏差*/
    

  33.     
    

  34.                 pid->pout = pid->p * pid->err;
    

  35.                 pid->iout += pid->i * pid->err;
    

  36.                 pid->dout = pid->d * (pid->err - pid->lastError);
    

  37.         
    

  38.                 abs_limit(&(pid->iout), pid->IntegralLimit);  /*积分限幅*/
    

  39.                 pid->pos_out = pid->pout + pid->iout + pid->dout;
    

  40.         
    

  41.                 abs_limit(&(pid->pos_out), pid->MaxOutput);  /*限定输出值的大小*/
    


  42. 

  43.     
    

  44.     /*更新数据*/
    

  45.     pid->lastError = pid->err;
    


  46. 

  47.     return pid->pos_out; /*PID输出*/
    

  48. }
    


  49.
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:04 | 显示全部楼层
pid.h
复制
  1. #ifndef __PID_H_
    

  2. #define __PID_H_
    


  3. 

  4. #include "main.h"
    


  5. 

  6. typedef struct __pid_t
    

  7. {
    

  8.     float p,i,d;
    

  9.     float err,lastError;        //误差
    


  10. 

  11.     float set;        //目标值
    

  12.     float get;        //测量值
    

  13.     
    

  14.     float pout;                //P输出                                
    

  15.     float iout;                //I输出                                        
    

  16.     float dout;                //D输出                
    

  17.     
    

  18.     float pos_out;                        //本次位置式输出，即 pos_out = pout + iout + dout
    

  19.     float last_pos_out;                //上次位置式输出
    

  20.     
    


  21. 

  22.     uint32_t MaxOutput;                        //输出限幅
    

  23.     uint32_t IntegralLimit;                //积分限幅
    

  24.     
    


  25. 

  26. }pid_t;
    


  27. 

  28. void abs_limit(float *a, float ABS_MAX);
    

  29. void PID_struct_init(pid_t* pid,
    

  30.                                                                                  uint32_t maxout,
    

  31.                                                                                  uint32_t intergral_limit,
    

  32.                                                                                  float         kp, 
    

  33.                                                                                  float         ki, 
    

  34.                                                                            float         kd);
    


  35. 

  36. float pid_calc(pid_t* pid, float get, float set);
    


  37. 

  38. #endif
    


  39.
回复 赏

举报

 楼主| yutingwei 发表于 2023-7-29 17:05 | 显示全部楼层
main.c
复制
  1. #include "encoder.h"
    

  2. #include "pid.h"
    


  3. 

  4. int main(void)
    

  5. {
    

  6.     HAL_Init();
    

  7.     SystemClock_Config();
    

  8.     MX_GPIO_Init();
    

  9.     MX_TIM1_Init();
    

  10.     MX_TIM2_Init();
    

  11.     MX_TIM6_Init();
    

  12.     /*以上为cube生成*/
    

  13.   
    

  14.         HAL_TIM_Base_Start_IT(&htim6);                       //开启1ms定时器中断
    

  15.         HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
    

  16.         Motor_Init();
    

  17.         
    

  18.         PID_struct_init(&Encoder_Motor_Pid_Pos[0], 100000, 1000, 0.2, 0.0, 0);
    

  19.         PID_struct_init(&Encoder_Motor_Pid_Spd[0], 1000, 1000, 30, 0.05, 0.01);
    


  20. 

  21.     while (1)
    

  22.     {
    

  23.                 SetCurrent(&encoderMoto[0], 20, SPEED);        
    

  24.     }
    

  25. }
回复 赏

举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

yutingwei

58

主题

514

帖子

0

粉丝
快速回复 在线客服 返回列表 返回顶部