stm32官方编码器例程pid实现

最近在调试编码器，用的貌似是官方给出的例程，但是在做pid的时候遇到困难，不知道该如何在此基础上实现pid，有大神指教吗？

#define ENCODER_TIMERTIM3// Encoder unit connected to TIM3
#define ENCODER_TIM_PERIOD (u16)32//line of encoder
#define COUNTER_RESET(u16)0
#define ICx_FILTER(u8) 6 // 6<-> 670nsec
static volatile u16 hEncoder_Timer_Overflow;
static u8 Duty_TIM3 = 0;
extern u16 hEncoder_Timer_Overflag;
u16 rotor_speed=0;
u8 Period_TIM3 = 0,CollecFlag_TIM3;

void ENC_Init(void)
{
TIM_TimeBaseInitTypeDefTIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
//TIM3 clock enable
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

//priority of TIM3
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);

GPIO_PinAFConfig(GPIOB,GPIO_PinSource4,GPIO_AF_TIM3);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource5,GPIO_AF_TIM3);


TIM_DeInit(ENCODER_TIMER);
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

TIM_TimeBaseStructure.TIM_Prescaler = 0X00;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = (4*ENCODER_TIM_PERIOD)-1;//éè??PWM?ü?ú
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInit(ENCODER_TIMER, &TIM_TimeBaseStructure);
//±à???÷?ó?ú?￡ê?3?ê??ˉ′?oˉêy?μ?÷??óD?¨ê±?÷μ?chanel1 and chanel2 ??óD±à???÷1|?ü￡?????í¨μà?a?ˉ3é±à???÷1|?ü
TIM_EncoderInterfaceConfig(ENCODER_TIMER, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);

//TIM_ICInitStructure.TIM_Channel=TIM_Channel_2;
//TIM_ICInitStructure.TIM_ICSelection=TIM_ICSelection_DirectTI;

TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;//??2¨ê±??è?o?????μ?à′￡?￡?
TIM_ICInit(ENCODER_TIMER, &TIM_ICInitStructure);

//TIM_SelectInputTrigger(ENCODER_TIMER, TIM_TS_TI2FP2); //????ê±?ó′￥·￠?′
//TIM_SelectSlaveMode(ENCODER_TIMER, TIM_SlaveMode_Reset);//′ó?￡ê?????
//TIM_SelectMasterSlaveMode(ENCODER_TIMER, TIM_MasterSlaveMode_Enable); //?÷′ó?￡ê?????

// Clear all pending interrupts
TIM_ClearFlag(ENCODER_TIMER,TIM_IT_CC1|TIM_FLAG_Update);
TIM_ITConfig(ENCODER_TIMER,TIM_IT_CC1|TIM_IT_Update, ENABLE);
//Reset counter
ENCODER_TIMER->CNT = COUNTER_RESET;

TIM_Cmd(ENCODER_TIMER, ENABLE);
}

s16 ENC_Get_Rotorspeed(void)
{
staticu16lastCount = 0;
u16curCount = ENCODER_TIMER->CNT;//
rotor_speed=(128*hEncoder_Timer_Overflag+curCount-lastCount)/64;
printf("hEncoder_Timer_Overflag=%drn",hEncoder_Timer_Overflag);
lastCount = curCount;
return (u16) rotor_speed;
}

void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //ò?3??D??
{
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);//??3y?D??±ê????
hEncoder_Timer_Overflag++;
}
else if(TIM_GetITStatus(TIM3,TIM_IT_CC1)==SET)
{
Duty_TIM3=TIM_GetCapture1(TIM3);
if(TIM_GetCapture2(TIM3)>64)
Period_TIM3=TIM_GetCapture2(TIM3);
CollecFlag_TIM3=0;
}
}