[嵌入式]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

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

2.3 Debug配置为Serial Wire模式

2.4 配置GPIO，PF0默认上拉

2.5 配置定时器

配置完成后生成代码

三、代码实现

• 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);	
    }
}