本篇来介绍电机的位置环控制，实现电机快速准确地转动到指定位置。
1 位置控制与速度控制的区别
回顾上篇，电机速度PID控制的结构图如下，目标值是设定的速度，通过编码器获取电机的转速作为反馈，实现电机转速的控制。
再来看电机位置PID控制，其结构图如下，目标值是设定的位置，通过编码器获取电机累计转动的脉冲数作为反馈，实现电机位置的控制。
所以：对比两张图，速度控制与位置控制的主要区别，就是控制量的不同。
2 核心程序
了解了速度控制与位置控制的区别后，下面就可以修改程序。
2.1 编码器相关
2.1.1 电机与编码器参数
编码器部分，需要根据自己电机的实际参数进行设定，比如我用到的电机：
- 编码器一圈的物理脉冲数为11
- 定时器编码器模式通过设置倍频来实现4倍频
- 电机的减速齿轮的减速比为1：34
所以，电机转一圈总的脉冲数，即定时器能读到的脉冲数为11*4*34= 1496。
#define ENCODER_RESOLUTION 11 /*编码器一圈的物理脉冲数*/
#define ENCODER_MULTIPLE 4 /*编码器倍频，通过定时器的编码器模式设置*/
#define MOTOR_REDUCTION_RATIO 34 /*电机的减速比*/
/*电机转一圈总的脉冲数(定时器能读到的脉冲数) = 编码器物理脉冲数*编码器倍频*电机减速比 */
/* 11*4*34= 1496*/
#define TOTAL_RESOLUTION ( ENCODER_RESOLUTION*ENCODER_MULTIPLE*MOTOR_REDUCTION_RATIO )
2.1.2 定时器编码器模式配置
用于编码器捕获的定时器的一些宏定义。
#define ENCODER_TIM_PSC 0 /*计数器分频*/
#define ENCODER_TIM_PERIOD 65535 /*计数器最大值*/
#define CNT_INIT 0 /*计数器初值*/
配置主要关注重装载值，倍频，溢出中断设置。
/* TIM4通道1通道2 正交编码器 */
void TIMx_encoder_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct; /*GPIO*/
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; /*时基*/
TIM_ICInitTypeDef TIM_ICInitStruct; /*输入通道*/
NVIC_InitTypeDef NVIC_InitStructure; /*中断*/
/*GPIO初始化*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);
/*时基初始化*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); /*使能定时器时钟 APB1*/
TIM_DeInit(TIM4);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC; /*预分频 */
TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD; /*周期(重装载值)*/
TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; /*连续向上计数模式*/
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStruct);
/*编码器模式配置：同时捕获通道1与通道2(即4倍频)，极性均为Rising*/
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 0; /*输入通道的滤波参数*/
TIM_ICInit(TIM4, &TIM_ICInitStruct); /*输入通道初始化*/
TIM_SetCounter(TIM4, CNT_INIT); /*CNT设初值*/
TIM_ClearFlag(TIM4,TIM_IT_Update); /*中断标志清0*/
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); /*中断使能*/
TIM_Cmd(TIM4,ENABLE); /*使能CR寄存器*/
/*中断配置*/
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
想要了解更多关于定时器编码器模式配置的详细介绍，可参照之前的文章：电机控制基础——定时器编码器模式使用与转速计算
2.1.3 读取编码器的值
读取值，这里直接读取原始值即可，读取后也不需要再设置计数初值，因为使用的溢出中断。
uint32_t read_encoder(void)
{
uint32_t encoderNum = 0;
encoderNum = (TIM4->CNT);
return encoderNum;
}
2.1.4 编码器计数值溢出处理
溢出中断中，主要判断是向上溢出还是向下溢出，因为电机可以正反转，所以需要记录溢出的方向。
/* 定时器溢出次数 */
__IO int16_t EncoderOverflowCnt = 0;
//定时器4中断服务函数
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //溢出中断
{
if((TIM4->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
{
EncoderOverflowCnt++;/*编码器计数值[向上]溢出*/
}
else
{
EncoderOverflowCnt--;/*编码器计数值[向下]溢出*/
}
}
TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //清除中断标志位
}
2.2 PID计算相关
2.2.1 周期定时
定时器配置，通过设置自动重装载值和定时器分频实现指定周期的定时。
void TIMx_calcPID_init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7,ENABLE); ///使能TIM7时钟
TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值
TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM7,&TIM_TimeBaseInitStructure);//初始化TIM7
TIM_ITConfig(TIM7,TIM_IT_Update,ENABLE); //允许定时器6更新中断
TIM_Cmd(TIM7,DISABLE); //初始化时先不开启定时器7
NVIC_InitStructure.NVIC_IRQChannel=TIM7_IRQn; //定时器6中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
TIMx_calcPID_init(100-1,8400-1);/*定时10ms,这句在主函数中调用*/
定时器中断中，每10ms进行1次PID计算
void TIM7_IRQHandler(void)
{
if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET) //溢出中断
{
AutoReloadCallback();
}
TIM_ClearITPendingBit(TIM7,TIM_IT_Update); //清除中断标志位
}
想要了解更多关于基础定时器的配置与使用，可参照之前的文章：电机控制基础——定时器基础知识与PWM输出原理
2.2.2 PID电机控制逻辑
周期定时器的回调函数中进行PID的计算，程序中被注释掉的两句是速度控制的代码，用于与位置控制进行对比，通过对比可以明显的看出，位置控制与速度控制的区别在于传入PID的控制量。
void AutoReloadCallback()
{
static __IO int encoderNow = 0; /*当前时刻总计数值*/
static __IO int encoderLast = 0; /*上一时刻总计数值*/
int encoderDelta = 0; /*当前时刻与上一时刻编码器的变化量*/
int res_pwm = 0; /*PID计算得到的PWM值*/
/*【1】读取编码器的值*/
encoderNow = read_encoder() + EncoderOverflowCnt*ENCODER_TIM_PERIOD;/*获取当前的累计值*/
encoderDelta = encoderNow - encoderLast; /*得到变化值*/
encoderLast = encoderNow;/*更新上次的累计值*/
/*【2】PID运算，得到PWM控制值*/
//res_pwm = pwm_val_protect((int)PID_realize(encoderDelta));/*传入编码器的[变化值]，实现电机【速度】控制*/
res_pwm = pwm_val_protect((int)PID_realize(encoderNow));/*传入编码器的[总计数值]，实现电机【位置】控制*/
/*【3】PWM控制电机*/
set_motor_rotate(res_pwm);
/*【4】数据上传到上位机显示*/
//set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderDelta, 1); /*给通道1发送实际的电机【速度】值*/
set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderNow, 1); /*给通道1发送实际的电机【位置】值*/
}
3 实验演示
实验中，指定目标值1496，可以实现电机正转1圈，再指定目标值-1496，因为是相对位置，电机会反转2圈。当指定14960转10圈时进行观察，若PID的参数不合适，会出现静态误差、或是持续抖动、或是误差消除慢等情况。通过不断的调整参数，可以实际感受到PID各项的调节作用。
本篇来介绍电机的位置环控制，实现电机快速准确地转动到指定位置。
1 位置控制与速度控制的区别
回顾上篇，电机速度PID控制的结构图如下，目标值是设定的速度，通过编码器获取电机的转速作为反馈，实现电机转速的控制。
再来看电机位置PID控制，其结构图如下，目标值是设定的位置，通过编码器获取电机累计转动的脉冲数作为反馈，实现电机位置的控制。
所以：对比两张图，速度控制与位置控制的主要区别，就是控制量的不同。
2 核心程序
了解了速度控制与位置控制的区别后，下面就可以修改程序。
2.1 编码器相关
2.1.1 电机与编码器参数
编码器部分，需要根据自己电机的实际参数进行设定，比如我用到的电机：
- 编码器一圈的物理脉冲数为11
- 定时器编码器模式通过设置倍频来实现4倍频
- 电机的减速齿轮的减速比为1：34
所以，电机转一圈总的脉冲数，即定时器能读到的脉冲数为11*4*34= 1496。
#define ENCODER_RESOLUTION 11 /*编码器一圈的物理脉冲数*/
#define ENCODER_MULTIPLE 4 /*编码器倍频，通过定时器的编码器模式设置*/
#define MOTOR_REDUCTION_RATIO 34 /*电机的减速比*/
/*电机转一圈总的脉冲数(定时器能读到的脉冲数) = 编码器物理脉冲数*编码器倍频*电机减速比 */
/* 11*4*34= 1496*/
#define TOTAL_RESOLUTION ( ENCODER_RESOLUTION*ENCODER_MULTIPLE*MOTOR_REDUCTION_RATIO )
2.1.2 定时器编码器模式配置
用于编码器捕获的定时器的一些宏定义。
#define ENCODER_TIM_PSC 0 /*计数器分频*/
#define ENCODER_TIM_PERIOD 65535 /*计数器最大值*/
#define CNT_INIT 0 /*计数器初值*/
配置主要关注重装载值，倍频，溢出中断设置。
/* TIM4通道1通道2 正交编码器 */
void TIMx_encoder_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct; /*GPIO*/
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; /*时基*/
TIM_ICInitTypeDef TIM_ICInitStruct; /*输入通道*/
NVIC_InitTypeDef NVIC_InitStructure; /*中断*/
/*GPIO初始化*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);
/*时基初始化*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); /*使能定时器时钟 APB1*/
TIM_DeInit(TIM4);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC; /*预分频 */
TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD; /*周期(重装载值)*/
TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; /*连续向上计数模式*/
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStruct);
/*编码器模式配置：同时捕获通道1与通道2(即4倍频)，极性均为Rising*/
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 0; /*输入通道的滤波参数*/
TIM_ICInit(TIM4, &TIM_ICInitStruct); /*输入通道初始化*/
TIM_SetCounter(TIM4, CNT_INIT); /*CNT设初值*/
TIM_ClearFlag(TIM4,TIM_IT_Update); /*中断标志清0*/
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); /*中断使能*/
TIM_Cmd(TIM4,ENABLE); /*使能CR寄存器*/
/*中断配置*/
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
想要了解更多关于定时器编码器模式配置的详细介绍，可参照之前的文章：电机控制基础——定时器编码器模式使用与转速计算
2.1.3 读取编码器的值
读取值，这里直接读取原始值即可，读取后也不需要再设置计数初值，因为使用的溢出中断。
uint32_t read_encoder(void)
{
uint32_t encoderNum = 0;
encoderNum = (TIM4->CNT);
return encoderNum;
}
2.1.4 编码器计数值溢出处理
溢出中断中，主要判断是向上溢出还是向下溢出，因为电机可以正反转，所以需要记录溢出的方向。
/* 定时器溢出次数 */
__IO int16_t EncoderOverflowCnt = 0;
//定时器4中断服务函数
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //溢出中断
{
if((TIM4->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
{
EncoderOverflowCnt++;/*编码器计数值[向上]溢出*/
}
else
{
EncoderOverflowCnt--;/*编码器计数值[向下]溢出*/
}
}
TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //清除中断标志位
}
2.2 PID计算相关
2.2.1 周期定时
定时器配置，通过设置自动重装载值和定时器分频实现指定周期的定时。
void TIMx_calcPID_init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7,ENABLE); ///使能TIM7时钟
TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值
TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM7,&TIM_TimeBaseInitStructure);//初始化TIM7
TIM_ITConfig(TIM7,TIM_IT_Update,ENABLE); //允许定时器6更新中断
TIM_Cmd(TIM7,DISABLE); //初始化时先不开启定时器7
NVIC_InitStructure.NVIC_IRQChannel=TIM7_IRQn; //定时器6中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
TIMx_calcPID_init(100-1,8400-1);/*定时10ms,这句在主函数中调用*/
定时器中断中，每10ms进行1次PID计算
void TIM7_IRQHandler(void)
{
if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET) //溢出中断
{
AutoReloadCallback();
}
TIM_ClearITPendingBit(TIM7,TIM_IT_Update); //清除中断标志位
}
想要了解更多关于基础定时器的配置与使用，可参照之前的文章：电机控制基础——定时器基础知识与PWM输出原理
2.2.2 PID电机控制逻辑
周期定时器的回调函数中进行PID的计算，程序中被注释掉的两句是速度控制的代码，用于与位置控制进行对比，通过对比可以明显的看出，位置控制与速度控制的区别在于传入PID的控制量。
void AutoReloadCallback()
{
static __IO int encoderNow = 0; /*当前时刻总计数值*/
static __IO int encoderLast = 0; /*上一时刻总计数值*/
int encoderDelta = 0; /*当前时刻与上一时刻编码器的变化量*/
int res_pwm = 0; /*PID计算得到的PWM值*/
/*【1】读取编码器的值*/
encoderNow = read_encoder() + EncoderOverflowCnt*ENCODER_TIM_PERIOD;/*获取当前的累计值*/
encoderDelta = encoderNow - encoderLast; /*得到变化值*/
encoderLast = encoderNow;/*更新上次的累计值*/
/*【2】PID运算，得到PWM控制值*/
//res_pwm = pwm_val_protect((int)PID_realize(encoderDelta));/*传入编码器的[变化值]，实现电机【速度】控制*/
res_pwm = pwm_val_protect((int)PID_realize(encoderNow));/*传入编码器的[总计数值]，实现电机【位置】控制*/
/*【3】PWM控制电机*/
set_motor_rotate(res_pwm);
/*【4】数据上传到上位机显示*/
//set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderDelta, 1); /*给通道1发送实际的电机【速度】值*/
set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderNow, 1); /*给通道1发送实际的电机【位置】值*/
}
3 实验演示
实验中，指定目标值1496，可以实现电机正转1圈，再指定目标值-1496，因为是相对位置，电机会反转2圈。当指定14960转10圈时进行观察，若PID的参数不合适，会出现静态误差、或是持续抖动、或是误差消除慢等情况。通过不断的调整参数，可以实际感受到PID各项的调节作用。
举报
