#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))
    {
        switch (mcFocCtrl.CtrlMode)
        {
            case 0:
            {
                if (motorControl.ActualSpeed > MOTOR_LOOP_RPM)
                {
                    Start_CNT ++;
                    
                    if (Start_CNT > 15)
                    {
                        mcFocCtrl.CtrlMode = 1;
                        Start_CNT = 0;
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        mcSpeedRamp.ActualValue = motorControl.ActualSpeed + _Q15(150 / MOTOR_SPEED_BASE);
                        #endif
                        mcFocCtrl.LoopTime = 1;
                        PI0_UKH = mcFocCtrl.mcIqref;
                        mcFocCtrl.IsRef = mcFocCtrl.mcIqref;
                    }
                }
            }
            break;
            
            case 1:
            {
                mcFocCtrl.LoopTime++;
                
                if (mcFocCtrl.LoopTime > SPEED_LOOP_TIME)
                {
                    #if (OUT_LOOP_CONTROL)
                    {
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        FOC_IQREF = HW_Zero_Calc(motorControl.TargetRef - motorControl.ActualSpeed);
                        #endif
                        
                    }
                    #else
                    {
                        FOC_IQREF = motorControl.QOutRef;
                    }
                    #endif
                    mcFocCtrl.LoopTime = 0;
                }
            }
            break;
        }
    }
}


/*---------------------------------------------------------------------------*/
/*  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.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;