代码收藏家技术教程 2023-03-27

“实现编码器电机速度和位置环闭环控制：使用STM32进行优化与美化”

此文章提供了一个通用的函数接口，仅需配置相关IO。基于Hal库开发。

一、硬件及接线说明

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定时器中断（无需接线）

二、CUBEMX配置

2.1 新建工程，配置时钟频率为72MHz

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-gjc8pXwn-1665843283351)(C:\Users\acer\AppData\Roaming\Typora\typora-user-images\image-20221015215919096.png)]$

2.2 配置RCC，使用外部高速晶振

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SWszCTgM-1665843142224)(C:\Users\acer\AppData\Roaming\Typora\typora-user-images\image-20221015220013874.png)]$

2.3 Debug配置为Serial Wire模式

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-phChakkt-1665843142224)(C:\Users\acer\AppData\Roaming\Typora\typora-user-images\image-20221015220126536.png)]$

2.4 配置GPIO，PF0默认上拉

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Ktwz0IPL-1665843142225)(C:\Users\acer\AppData\Roaming\Typora\typora-user-images\image-20221015220213294.png)]$

2.5 配置定时器

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-i9nbJTx8-1665843142226)(C:\Users\acer\AppData\Roaming\Typora\typora-user-images\image-20221015220324625.png)]$

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sOMyMyKV-1665843142227)(C:\Users\acer\AppData\Roaming\Typora\typora-user-images\image-20221015220623168.png)]$

配置完成后生成代码

三、代码实现

encoder.c

/* 此文件为编码器电机闭环调试，包括速度环和位置环
 * 配置：TIM2(PA0、PA1)：编码器模式
 * 			 TIM1-CH1(PE9)：PWM输出
 *			 IN1：PF1
 * 			 IN2: PF2
 * 	     ENABLE: PF0
 * 使用方法：1、初始化Motor_Init()
 * 					 2、发送电流SetCurrent()
 */
#include  "encoder.h"

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

pid_t Encoder_Motor_Pid_Pos[ENCODER_MOTO_COUNT];  //编码器电机位置环PID结构体
pid_t Encoder_Motor_Pid_Spd[ENCODER_MOTO_COUNT];  //编码器电机速度环PID结构体


void Motor_Init(void)
{
		HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);      //开启编码器定时器
		__HAL_TIM_ENABLE_IT(&htim2,TIM_IT_UPDATE);         	 //开启编码器定时器更新中断,防溢出处理
		__HAL_TIM_SET_COUNTER(&htim2, 10000);                //编码器定时器初始值设定为10000
	
		encoderMoto[0].IOControl.htim_pwm = htim1;
		encoderMoto[0].IOControl.IN1_Port = GPIOF;
		encoderMoto[0].IOControl.IN2_Port = GPIOF;
		encoderMoto[0].IOControl.IN1_Pin = GPIO_PIN_1;
		encoderMoto[0].IOControl.IN2_Pin = GPIO_PIN_2;
}


//
/* 编码器电机发送电流函数
 * motor：编码器电机参数结构体
 * val：转动的速度或角度，SPEED最大为110，POSITION一圈为3960
 * mode：模式选择：速度环：SPEED
 * 								 位置环：POSITION
 */
void SetCurrent(encoderMotor_t *motor, int32_t val, uint32_t mode)
{
		float pos_output,spd_output;
		if(mode == 1)
			spd_output = pid_calc(&Encoder_Motor_Pid_Spd[0], motor->speed, val);
		else
		{
			pos_output = pid_calc(&Encoder_Motor_Pid_Pos[0], motor->totalAngle, val);
			spd_output = pid_calc(&Encoder_Motor_Pid_Spd[0], motor->speed, pos_output);
		}

		if(spd_output > 0)
		{
				HAL_GPIO_WritePin(motor->IOControl.IN1_Port, motor->IOControl.IN1_Pin, GPIO_PIN_SET);		//控制正反转
				HAL_GPIO_WritePin(motor->IOControl.IN2_Port, motor->IOControl.IN2_Pin, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(&motor->IOControl.htim_pwm, TIM_CHANNEL_1, (uint32_t)(spd_output));
		}
		else
		{
				HAL_GPIO_WritePin(motor->IOControl.IN1_Port, motor->IOControl.IN1_Pin, GPIO_PIN_RESET);
				HAL_GPIO_WritePin(motor->IOControl.IN2_Port, motor->IOControl.IN2_Pin, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(&motor->IOControl.htim_pwm, TIM_CHANNEL_1, (uint32_t)(-spd_output));
		}
	
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	static int16_t count = 0;
	if(htim->Instance==htim6.Instance)		         //1ms中断
	{
		count++;
		if(count >= 10)
		{
			count = 0;
			int16_t pluse = COUNTERNUM - RELOADVALUE/2;											
			encoderMoto[0].totalAngle = pluse + encoderMoto[0].loopNum * RELOADVALUE/2;  
			encoderMoto[0].speed = (float)(encoderMoto[0].totalAngle - encoderMoto[0].lastAngle)/(4*PLUSE_OF_CIRCLE*RR)*6000;			//进行速度计算,根据前文所说的,4倍频,编码器13位,减速比30,再乘以6000即为每分钟输出轴多少转
			encoderMoto[0].lastAngle = encoderMoto[0].totalAngle;         //更新转过的圈数
		}
	}
	else if(htim->Instance == htim2.Instance)      //如果是编码器更新中断,即10ms内,脉冲数超过了计数范围,需要进行处理
	{
		if(COUNTERNUM < 10000)	encoderMoto[0].loopNum++;
		else if(COUNTERNUM > 10000)	encoderMoto[0].loopNum--;
		__HAL_TIM_SetCounter(&htim2, 10000);             //重新设定初始值			
	}
}

encoder.h

#ifndef __ENCODER_H
#define __ENCODER_H

#include "tim.h"
#include "gpio.h"
#include "main.h"
#include "stm32_hal_legacy.h"
#include "pid.h"

#define ENCODER_MOTO_COUNT		1		//编码器电机数量
#define RR	90	//电机减速比
#define PLUSE_OF_CIRCLE		11
#define RELOADVALUE 	__HAL_TIM_GetAutoreload(&htim2)    //获取自动装载值,本例中为20000
#define COUNTERNUM 		__HAL_TIM_GetCounter(&htim2)        //获取编码器定时器中的计数值


#define MOTOR_1		1

enum{
    POSITION	= 0,
    SPEED 	= 1,
};

/* 编码器电机接口定义结构体 */
typedef struct _IOControl
{
	TIM_HandleTypeDef htim_encoder;
	TIM_HandleTypeDef htim_pwm;
	GPIO_TypeDef *IN1_Port;
	GPIO_TypeDef *IN2_Port;
	uint16_t IN1_Pin;
	uint16_t IN2_Pin;
}IOControl_t;

/* 编码器电机参数结构体 */
typedef struct _EncoderMotor{
	int8_t ID;
	int16_t loopNum;          //防超上限
	int32_t lastAngle;        //上1ms转的角度
	int32_t totalAngle;       //总角度
	float speed;              //电机输出轴转速，单位RPM
	float set;
	IOControl_t IOControl;
}encoderMotor_t;

extern encoderMotor_t encoderMoto[ENCODER_MOTO_COUNT];
extern pid_t Encoder_Motor_Pid_Pos[ENCODER_MOTO_COUNT];  //编码器电机位置环PID结构体
extern pid_t Encoder_Motor_Pid_Spd[ENCODER_MOTO_COUNT];  //编码器电机速度环PID结构体

void SetCurrent(encoderMotor_t *motor, int32_t val, uint32_t mode);
void Motor_Init(void);
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim);

#endif

pid.c

#include "pid.h"


void abs_limit(float *a, float ABS_MAX)
{
    if(*a > ABS_MAX)
        *a = ABS_MAX;
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
}

void PID_struct_init(pid_t* pid,
										 uint32_t maxout,
										 uint32_t intergral_limit,
										 float 	kp, 
										 float 	ki, 
									   float 	kd)
{
    pid->IntegralLimit = intergral_limit;
    pid->MaxOutput = maxout;
    
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
}


float pid_calc(pid_t* pid, float get, float set)
{
    pid->get = get;
    pid->set = set;
    pid->err = set - get;	/*set - measure，得到偏差*/
    
		pid->pout = pid->p * pid->err;
		pid->iout += pid->i * pid->err;
		pid->dout = pid->d * (pid->err - pid->lastError);
	
		abs_limit(&(pid->iout), pid->IntegralLimit);  /*积分限幅*/
		pid->pos_out = pid->pout + pid->iout + pid->dout;
	
		abs_limit(&(pid->pos_out), pid->MaxOutput);  /*限定输出值的大小*/

    
    /*更新数据*/
    pid->lastError = pid->err;

    return pid->pos_out; /*PID输出*/
}

pid.h

#ifndef __PID_H_
#define __PID_H_

#include "main.h"

typedef struct __pid_t
{
    float p,i,d;
    float err,lastError;	//误差

    float set;	//目标值
    float get;	//测量值
    
    float pout;		//P输出				
    float iout;		//I输出					
    float dout;		//D输出		
    
    float pos_out;			//本次位置式输出，即 pos_out = pout + iout + dout
    float last_pos_out;		//上次位置式输出
    

    uint32_t MaxOutput;			//输出限幅
    uint32_t IntegralLimit;		//积分限幅
    

}pid_t;

void abs_limit(float *a, float ABS_MAX);
void PID_struct_init(pid_t* pid,
										 uint32_t maxout,
										 uint32_t intergral_limit,
										 float 	kp, 
										 float 	ki, 
									   float 	kd);

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

#endif

main.c

#include "encoder.h"
#include "pid.h"

int main(void)
{
    HAL_Init();
    SystemClock_Config();
    MX_GPIO_Init();
    MX_TIM1_Init();
    MX_TIM2_Init();
    MX_TIM6_Init();
    /*以上为cube生成*/
  
	HAL_TIM_Base_Start_IT(&htim6);                       //开启1ms定时器中断
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	Motor_Init();
	
	PID_struct_init(&Encoder_Motor_Pid_Pos[0], 100000, 1000, 0.2, 0.0, 0);
	PID_struct_init(&Encoder_Motor_Pid_Spd[0], 1000, 1000, 30, 0.05, 0.01);

    while (1)
    {
		SetCurrent(&encoderMoto[0], 20, SPEED);	
    }
}