예제 #1
0
//***********************************速度PI***********************************************
void SpeedControl(void) 
 {
    float fP,fI,fDelta;
    int8 index;
    
    if(g_nSpeedControlFlag == 0) 
     {
        g_fSpeedControlOutOld =0; 
        g_fSpeedControlOutNew =0;
        g_fSpeedControlOut = 0;
        g_fSpeedControlIntegral = 0;
        return;
     }
    //-----------------------------------------------------------------------
    g_nLeftMotorPulse  =  LPLD_FTM_GetCounter(FTM2);
    g_nRightMotorPulse =  LPLD_FTM_GetCounter(FTM1);
    LPLD_FTM_ClearCounter(FTM2);
    LPLD_FTM_ClearCounter(FTM1);
    //if(g_nLeftMotorPulse<0)		g_nLeftMotorPulse = -g_nLeftMotorPulse;
    //if(g_nRightMotorPulse<0)		g_nRightMotorPulse = -g_nRightMotorPulse;
    
    g_fCarSpeed = (g_nLeftMotorPulse + g_nRightMotorPulse) / 2;
    //------------------------------------------------------------------------
    
    fDelta =CarSpeedTarget-g_fCarSpeed;
   
    //@@@@@@@@@@@@@@@@@@@@===16.5.17 night====@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    if( fabs(fDelta)<180 )
    {
      index=2;
    }else index=1;
     //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    
    fP = fDelta * SPEED_CONTROL_P*0.01;
    fI = fDelta * SPEED_CONTROL_I*index*0.01;
    
    g_fSpeedControlIntegral += fI;
    
    //==========================================================================
#define INTEGRAL_MAX        3000
#define INTEGRAL_MIN       -3000   
    
#define P_MAX         3000    
#define P_MIN        -3000
    
    if (g_fSpeedControlIntegral >= INTEGRAL_MAX)     g_fSpeedControlIntegral=INTEGRAL_MAX;
    if (g_fSpeedControlIntegral <= INTEGRAL_MIN)    g_fSpeedControlIntegral=-INTEGRAL_MIN;
    
    if (fP >= P_MAX )     fP=P_MAX;
    if (fP <= P_MIN )     fP=P_MIN;
    
    //===========================================================================    
    g_fSpeedControlOutOld = g_fSpeedControlOutNew;
    g_fSpeedControlOutNew = fP + g_fSpeedControlIntegral;
    
 }
/*
 * PIT中断函数
 */
void pit_isr(void)
{
  //获取FTM1的正交解码计数值
  qd_result = LPLD_FTM_GetCounter(FTM1);
  //清空计数器
  LPLD_FTM_ClearCounter(FTM1);
}