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

 #define ENCODER_TIMER   TIM3  // 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_TimeBaseInitTypeDef  TIM_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);
        //±àÂëÆ÷½Ó¿Úģʽ³õʼ»¯    ´Ëº¯Êý˵Ã÷Ö»Óж¨Ê±Æ÷µÄchanel1 and chanel2 ¾ßÓбàÂëÆ÷¹¦ÄÜ£¬ÆäËûͨµÀΪ¼¯³É±àÂëÆ÷¹¦ÄÜ
         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;//Â˲¨Ê±¼äÈçºÎ¼ÆËãµÃÀ´£¿£¿
        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)
{
    static  u16   lastCount = 0;
    u16  curCount = ENCODER_TIMER->CNT;//
      rotor_speed=(128*hEncoder_Timer_Overflag+curCount-lastCount)/64;
      printf("hEncoder_Timer_Overflag=%d\r\n",hEncoder_Timer_Overflag);
      lastCount = curCount;
      return (u16) rotor_speed;
}

void TIM3_IRQHandler(void)
{
    if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //Òç³öÖжÏ
    {
        TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  //Çå³ýÖжϱê־λ
        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;
    }
}