AC-Motor-FU6815/User/Function/AddFunction.c

#include <Myproject.h>

extern uint16 SKI1, SKP1;

CurrentVarible     xdata   mcCurVarible;
FOCCTRL            xdata   mcFocCtrl;
MCRAMP             xdata   mcSpeedRamp;


void FaultProcess(void)
{
    PRE_DRIVER_RST = 0;
    ClrBit(DRV_CR, FOCEN);
    MOE = 0;
}


/*---------------------------------------------------------------------------*/
/*  Name     :   int32 Abs_F32(int32 value)
    /* Input    :   value
    /* Output   :   int16
    /* Description: 对变量取16位的绝对值
    /*---------------------------------------------------------------------------*/
uint32 Abs_F32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void Speed_response(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制、恒功率控制
    /*---------------------------------------------------------------------------*/
void Speed_response(void)
{
    static uint8 Start_CNT = 0;
    
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        if (mcFocCtrl.RunStateCnt < MotorStartHoldTime)                                                 // MotorStartHoldTime目标转速为启动转速
        {
            mcSpeedRamp.IncValue    = SpeedRampStartInc;
            mcFocCtrl.RunStateCnt++;
            
            if (isCtrlPowOn)
            {
                motorControl.TargetRef = Motor_Start_Hold_Speed;                    //上油时间内维持上油转速
            }
            else
            {
                motorControl.TargetRef = 0;
                mcSpeedRamp.DecValue = Motor_Speed_Dec1;
            }
        }
        else                                                                                                                            //如果达到上油转速后，将起动速度环增量变为运行速度环增量
        {
            Current_Speed_PI();
            mcSpeedRamp.IncValue = Motor_Speed_Inc;
            
            if ((!isCtrlPowOn) || (motorControl.TargetRef == 0))
            {
                mcSpeedRamp.DecValue = Motor_Speed_Dec1;
            }
            else
            {
                mcSpeedRamp.DecValue = Motor_Speed_Dec;
            }
        }
        
        switch (mcFocCtrl.CtrlMode)
        {
            case 0:
            {
                if (motorControl.ActualSpeed > MOTOR_LOOP_RPM)
                {
                    if (Start_CNT < 15)
                    {
                        Start_CNT ++;
                    }
                    else
                    {
                        mcFocCtrl.CtrlMode = 1;
                        Start_CNT = 0;
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        mcSpeedRamp.ActualValue = motorControl.ActualSpeed + _Q15(150 / MOTOR_SPEED_BASE);
                        #endif
                        mcFocCtrl.LoopTime = SPEED_LOOP_TIME;
                        mcFocCtrl.IND_DEC_LoopTime = SPEED_INC_DEC_TIME;
                        PI0_UKH = mcFocCtrl.mcIqref;
                        mcFocCtrl.IsRef    = mcFocCtrl.mcIqref;
                    }
                }
            }
            break;
            
            case 1:
            {
                mcFocCtrl.LoopTime++;
                mcFocCtrl.IND_DEC_LoopTime++;
                
                if (mcFocCtrl.IND_DEC_LoopTime > SPEED_INC_DEC_TIME)
                {
                    mcSpeedRamp.TargetValue = motorControl.TargetRef;
                    mc_ramp(&mcSpeedRamp);
                    mcFocCtrl.IND_DEC_LoopTime = 0;
                }
                
                if (mcFocCtrl.LoopTime > SPEED_LOOP_TIME)
                {
                    #if (OUT_LOOP_CONTROL)
                    {
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        mcFocCtrl.IsRef = HW_Zero_Calc(mcSpeedRamp.ActualValue - motorControl.ActualSpeed);
                        #endif
                        
                        STT_FOC_THECOMP_CLEAR();
                    }
                    #else
                    {
                        mcFocCtrl.mcIqref = FOC_IQREF;
                    
                        if (FOC_IQREF < motorControl.QOutRef)
                        {
                            mcFocCtrl.mcIqref += QOUTINC;
                    
                            if (mcFocCtrl.mcIqref > motorControl.QOutRef)
                            { mcFocCtrl.mcIqref = motorControl.QOutRef; }
                    
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        else if (FOC_IQREF > motorControl.QOutRef)
                        {
                            mcFocCtrl.mcIqref -= QOUTINC;
                    
                            if (mcFocCtrl.mcIqref < motorControl.QOutRef)
                            { mcFocCtrl.mcIqref = motorControl.QOutRef; }
                    
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                    }
                    #endif
                    mcFocCtrl.LoopTime = 0;
                }
            }
            break;
        }
    }
}


/*角度补偿清零*/
void STT_FOC_THECOMP_CLEAR(void)
{
    mcFocCtrl.foc_comp_temp = mcFocCtrl.STT_FOC_THECOMP;
    
    if (mcFocCtrl.foc_comp_temp < 0)
    {
        mcFocCtrl.foc_comp_temp++;
        mcFocCtrl.STT_FOC_THECOMP    =  mcFocCtrl.foc_comp_temp;
    }
    else if (mcFocCtrl.foc_comp_temp > 0)
    {
        mcFocCtrl.foc_comp_temp--;
        mcFocCtrl.STT_FOC_THECOMP    =  mcFocCtrl.foc_comp_temp;
    }
    else
    {
        mcFocCtrl.foc_comp_temp = 0;
        mcFocCtrl.STT_FOC_THECOMP       = 0;
    }
    
    FOC_THECOMP = mcFocCtrl.STT_FOC_THECOMP;
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void mc_ramp(void)
    /* Input    :   hTarget,MC_RAMP *hSpeedramp
    /* Output   :   NO
    /* Description:
    /*---------------------------------------------------------------------------*/

void mc_ramp(MCRAMP * hSpeedramp)
{
    if (hSpeedramp->ActualValue < hSpeedramp->TargetValue)
    {
        if (hSpeedramp->ActualValue + hSpeedramp->IncValue < hSpeedramp->TargetValue)
        {
            hSpeedramp->ActualValue += hSpeedramp->IncValue;
        }
        else
        {
            hSpeedramp->ActualValue = hSpeedramp->TargetValue;
        }
    }
    else
    {
        if (hSpeedramp->ActualValue - hSpeedramp->DecValue > hSpeedramp->TargetValue)
        {
            hSpeedramp->ActualValue -= hSpeedramp->DecValue;
        }
        else
        {
            hSpeedramp->ActualValue = hSpeedramp->TargetValue;
        }
    }
}


/*---------------------------------------------------------------------------*/
/*  Name     :   void StarRampDealwith(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description:
    /*---------------------------------------------------------------------------*/

void StarRampDealwith(void)
{
    if ((mcState == mcRun) || (mcState == mcStart))
    {
        if ((mcFocCtrl.CtrlMode == 0) || (mcFocCtrl.State_Count > 10))
        {
            if (mcFocCtrl.iiqq <= IQ_Start_CURRENT2)
            {mcFocCtrl.iiqq = mcFocCtrl.iiqq + 2;}
            
            FOC_IQREF = mcFocCtrl.iiqq;
        }
        
        if (mcFocCtrl.State_Count == 1200) //2300
        {
            FOC_EKP = OBSW_KP_GAIN_RUN;              // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN;              // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN2;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN2;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 1000) //2000
        {
            FOC_EKP = OBSW_KP_GAIN_RUN1;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN3;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN3;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 600) //1600
        {
            FOC_EKP = OBSW_KP_GAIN_RUN2;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN4;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN4;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 400) //1200
        {
            FOC_EKP = OBSW_KP_GAIN_RUN3;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN3;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN5;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN5;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 300)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN4;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN5;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN5;
            }
            #endif
        }
    }
}


/*PI参数分段处理*/
void Current_Speed_PI(void)
{
    static int16 Skp, Ski, DQkp, DQki;
    static int16 Skp_1, Ski_1, DQkp_1, DQki_1;
    
    if (motorControl.ActualSpeed < _Q15(2000 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun;
        Ski = SKIRun;
        DQkp = DQKP;
        DQki = DQKI;
    }
    else if (motorControl.ActualSpeed < _Q15(4200 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun1;
        Ski = SKIRun1;
        DQkp = DQKP_zhong;
        DQki = DQKI_zhong;
    }
    else if (motorControl.ActualSpeed > _Q15(4700 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun2;
        Ski = SKIRun2;
        DQkp = DQKP_highspeed;
        DQki = DQKI_highspeed;
    }
    
    Skp_1 = PI0_KP;
    Ski_1 = PI0_KI;
    
    if (Skp_1 < Skp)
    {
        Skp_1 += 2;
    }
    else if (Skp_1 > Skp)
    {
        Skp_1 -= 2;
    }
    
    if (Ski_1 < Ski)
    {
        Ski_1 += 2;
    }
    else if (Ski_1 > Ski)
    {
        Ski_1 -= 2;
    }
    
    PI0_KP = Skp_1;
    PI0_KI = Ski_1;
    DQkp_1 = FOC_QKP;
    DQki_1 = FOC_QKI;
    
    if (DQkp_1 < DQkp)
    {
        DQkp_1 += 2;
    }
    else if (DQkp_1 > DQkp)
    {
        DQkp_1 -= 2;
    }
    
    if (DQki_1 < DQki)
    {
        DQki_1 += 2;
    }
    else if (DQki_1 > DQki)
    {
        DQki_1 -= 2;
    }
    
    FOC_DKP = DQkp_1;
    FOC_DKI = DQki_1;
    FOC_QKP = DQkp_1;
    FOC_QKI = DQki_1;
}

uint16 AimFrequencyMaxVAC = 500;