電機(jī)控制進(jìn)階2——PID位置控制

上篇文章講解了電機(jī)的速度環(huán)控制，可以控制電機(jī)快速準(zhǔn)確地到達(dá)指定速度。

本篇來(lái)介紹電機(jī)的位置環(huán)控制，實(shí)現(xiàn)電機(jī)快速準(zhǔn)確地轉(zhuǎn)動(dòng)到指定位置。

1 位置控制與速度控制的區(qū)別

回顧上篇，電機(jī)速度PID控制的結(jié)構(gòu)圖如下，目標(biāo)值是設(shè)定的速度，通過(guò)編碼器獲取電機(jī)的轉(zhuǎn)速作為反饋，實(shí)現(xiàn)電機(jī)轉(zhuǎn)速的控制。

再來(lái)看電機(jī)位置PID控制，其結(jié)構(gòu)圖如下，目標(biāo)值是設(shè)定的位置，通過(guò)編碼器獲取電機(jī)累計(jì)轉(zhuǎn)動(dòng)的脈沖數(shù)作為反饋，實(shí)現(xiàn)電機(jī)位置的控制。

所以：對(duì)比兩張圖，速度控制與位置控制的主要區(qū)別，就是控制量的不同。

2 核心程序

了解了速度控制與位置控制的區(qū)別后，下面就可以修改程序。

2.1 編碼器相關(guān)

2.1.1 電機(jī)與編碼器參數(shù)

編碼器部分，需要根據(jù)自己電機(jī)的實(shí)際參數(shù)進(jìn)行設(shè)定，比如我用到的電機(jī)：

編碼器一圈的物理脈沖數(shù)為11

定時(shí)器編碼器模式通過(guò)設(shè)置倍頻來(lái)實(shí)現(xiàn)4倍頻

電機(jī)的減速齒輪的減速比為1：34

所以，電機(jī)轉(zhuǎn)一圈總的脈沖數(shù)，即定時(shí)器能讀到的脈沖數(shù)為11*4*34= 1496。

#define ENCODER_RESOLUTION 11  

/*編碼器一圈的物理脈沖數(shù)*/

#define ENCODER_MULTIPLE 4    

/*編碼器倍頻，通過(guò)定時(shí)器的編碼器模式設(shè)置*/

#define MOTOR_REDUCTION_RATIO 34 

/*電機(jī)的減速比*/

?

/*電機(jī)轉(zhuǎn)一圈總的脈沖數(shù)(定時(shí)器能讀到的脈沖數(shù)) = 編碼器物理脈沖數(shù)*編碼器倍頻*電機(jī)減速比 */

/* 11*4*34= 1496*/

#define TOTAL_RESOLUTION ( ENCODER_RESOLUTION*ENCODER_MULTIPLE*MOTOR_REDUCTION_RATIO ) 

想要了解更多關(guān)于編碼器的使用，可參照之前的文章: ( http://www.ttokpm.com/d/1639052.html )

2.1.2 定時(shí)器編碼器模式配置

用于編碼器捕獲的定時(shí)器的一些宏定義。

#define ENCODER_TIM_PSC  0     

/*計(jì)數(shù)器分頻*/

#define ENCODER_TIM_PERIOD  65535  

/*計(jì)數(shù)器最大值*/

#define CNT_INIT 0         

/*計(jì)數(shù)器初值*/

配置主要關(guān)注重裝載值，倍頻，溢出中斷設(shè)置。

/* TIM4通道1通道2 正交編碼器 */
void TIMx_encoder_init(void)                      
{ 
	GPIO_InitTypeDef GPIO_InitStruct;            /*GPIO*/
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStruct; /*時(shí)基*/
	TIM_ICInitTypeDef TIM_ICInitStruct;          /*輸入通道*/
	NVIC_InitTypeDef NVIC_InitStructure;         /*中斷*/
    
    /*GPIO初始化*/    
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); /*使能GPIO時(shí)鐘 AHB1*/                    
	GPIO_StructInit(&GPIO_InitStruct);        
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; 
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;        /*復(fù)用功能*/
	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); 

	/*時(shí)基初始化*/
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);   /*使能定時(shí)器時(shí)鐘 APB1*/
	TIM_DeInit(TIM4);  
	TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);    
	TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC;       /*預(yù)分頻 */        
	TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD;       /*周期(重裝載值)*/
	TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;      
	TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up;  /*連續(xù)向上計(jì)數(shù)模式*/  
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStruct); 

	/*編碼器模式配置：同時(shí)捕獲通道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;   /*輸入通道的濾波參數(shù)*/
	TIM_ICInit(TIM4, &TIM_ICInitStruct); /*輸入通道初始化*/
	TIM_SetCounter(TIM4, CNT_INIT);      /*CNT設(shè)初值*/
	TIM_ClearFlag(TIM4,TIM_IT_Update);   /*中斷標(biāo)志清0*/
	TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); /*中斷使能*/
	TIM_Cmd(TIM4,ENABLE);                /*使能CR寄存器*/
	
	/*中斷配置*/
	NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定時(shí)器4中斷
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //搶占優(yōu)先級(jí)1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子優(yōu)先級(jí)1
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
} 

想要了解更多關(guān)于定時(shí)器編碼器模式配置的詳細(xì)介紹，可參照之前的文章：

2.1.3 讀取編碼器的值

讀取值，這里直接讀取原始值即可，讀取后也不需要再設(shè)置計(jì)數(shù)初值，因?yàn)槭褂玫囊绯鲋袛唷?/p>

uint32_t read_encoder(void)
{
	uint32_t encoderNum = 0;
	encoderNum = (TIM4->CNT); 
	return encoderNum;
}

2.1.4 編碼器計(jì)數(shù)值溢出處理

溢出中斷中，主要判斷是向上溢出還是向下溢出，因?yàn)殡姍C(jī)可以正反轉(zhuǎn)，所以需要記錄溢出的方向。

/* 定時(shí)器溢出次數(shù) */
__IO int16_t EncoderOverflowCnt = 0;

//定時(shí)器4中斷服務(wù)函數(shù)
void TIM4_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //溢出中斷
	{
		if((TIM4->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
		{
			EncoderOverflowCnt++;/*編碼器計(jì)數(shù)值[向上]溢出*/
		}
		else
		{
			EncoderOverflowCnt--;/*編碼器計(jì)數(shù)值[向下]溢出*/
		}
	}
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update);  //清除中斷標(biāo)志位
}

2.2 PID計(jì)算相關(guān)

2.2.1 周期定時(shí)

定時(shí)器配置，通過(guò)設(shè)置自動(dòng)重裝載值和定時(shí)器分頻實(shí)現(xiàn)指定周期的定時(shí)。

void TIMx_calcPID_init(u16 arr,u16 psc)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7,ENABLE);  ///使能TIM7時(shí)鐘
	
    TIM_TimeBaseInitStructure.TIM_Period = arr;   //自動(dòng)重裝載值
	TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定時(shí)器分頻
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上計(jì)數(shù)模式
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
	TIM_TimeBaseInit(TIM7,&TIM_TimeBaseInitStructure);//初始化TIM7
	
	TIM_ITConfig(TIM7,TIM_IT_Update,ENABLE); //允許定時(shí)器6更新中斷
	TIM_Cmd(TIM7,DISABLE); //初始化時(shí)先不開(kāi)啟定時(shí)器7
	
	NVIC_InitStructure.NVIC_IRQChannel=TIM7_IRQn; //定時(shí)器6中斷
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //搶占優(yōu)先級(jí)1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子優(yōu)先級(jí)3
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}

TIMx_calcPID_init(100-1,8400-1);/*定時(shí)10ms,這句在主函數(shù)中調(diào)用*/

定時(shí)器中斷中，每10ms進(jìn)行1次PID計(jì)算

void TIM7_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET) //溢出中斷
	{
		AutoReloadCallback();
	}
	TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中斷標(biāo)志位
}

想要了解更多關(guān)于基礎(chǔ)定時(shí)器的配置與使用，可參照之前的文章：( http://www.ttokpm.com/d/1640145.html )

2.2.2 PID電機(jī)控制邏輯

周期定時(shí)器的回調(diào)函數(shù)中進(jìn)行PID的計(jì)算，程序中被注釋掉的兩句是速度控制的代碼，用于與位置控制進(jìn)行對(duì)比，通過(guò)對(duì)比可以明顯的看出，位置控制與速度控制的區(qū)別在于傳入PID的控制量。

void AutoReloadCallback()
{
	static __IO int encoderNow = 0;    /*當(dāng)前時(shí)刻總計(jì)數(shù)值*/
	static __IO int encoderLast = 0;   /*上一時(shí)刻總計(jì)數(shù)值*/
	int encoderDelta = 0; /*當(dāng)前時(shí)刻與上一時(shí)刻編碼器的變化量*/
	int res_pwm = 0; /*PID計(jì)算得到的PWM值*/

    /*【1】讀取編碼器的值*/
    encoderNow = read_encoder() + EncoderOverflowCnt*ENCODER_TIM_PERIOD;/*獲取當(dāng)前的累計(jì)值*/
    encoderDelta = encoderNow - encoderLast; /*得到變化值*/
    encoderLast = encoderNow;/*更新上次的累計(jì)值*/

    /*【2】PID運(yùn)算，得到PWM控制值*/
    //res_pwm = pwm_val_protect((int)PID_realize(encoderDelta));/*傳入編碼器的[變化值]，實(shí)現(xiàn)電機(jī)【速度】控制*/
    res_pwm = pwm_val_protect((int)PID_realize(encoderNow));/*傳入編碼器的[總計(jì)數(shù)值]，實(shí)現(xiàn)電機(jī)【位置】控制*/

    /*【3】PWM控制電機(jī)*/
    set_motor_rotate(res_pwm);

    /*【4】數(shù)據(jù)上傳到上位機(jī)顯示*/
    //set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderDelta, 1); /*給通道1發(fā)送實(shí)際的電機(jī)【速度】值*/
    set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderNow, 1); /*給通道1發(fā)送實(shí)際的電機(jī)【位置】值*/

}

3 實(shí)驗(yàn)演示

實(shí)驗(yàn)中，指定目標(biāo)值1496，可以實(shí)現(xiàn)電機(jī)正轉(zhuǎn)1圈，再指定目標(biāo)值-1496，因?yàn)槭窍鄬?duì)位置，電機(jī)會(huì)反轉(zhuǎn)2圈。當(dāng)指定14960轉(zhuǎn)10圈時(shí)進(jìn)行觀(guān)察，若PID的參數(shù)不合適，會(huì)出現(xiàn)靜態(tài)誤差、或是持續(xù)抖動(dòng)、或是誤差消除慢等情況。通過(guò)不斷的調(diào)整參數(shù)，可以實(shí)際感受到PID各項(xiàng)的調(diào)節(jié)作用。