stm32官方编码器例程pid实现

最近在调试编码器，用的貌似是官方给出的例程，但是在做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);
        /**
         *òy???′ó?    μ?μ×ê??′ó??￡ê?
         **/
        GPIO_PinAFConfig(GPIOB,GPIO_PinSource4,GPIO_AF_TIM3);
        GPIO_PinAFConfig(GPIOB,GPIO_PinSource5,GPIO_AF_TIM3);
        
         /* Timer configuration in Encoder mode */
        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)
{
    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) //ò?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;
    }
}
采用的是中断溢出的方式进行速度计算的，关于中断服务函数是参考资料写的，但是实现PID好像还有不少的问题、阿莫西论坛
哪位能帮我把阿莫西论坛的源码下载下来。小弟谢谢了先。源码可以发生到俅俅：605172128

小编   做出来了吗   求指教