#include <Myproject.h>

extern uint16 SKI1, SKP1;
/* Private variables ---------------------------------------------------------*/
PWMINPUTCAL        xdata   mcPwmInput;
CurrentVarible     xdata   mcCurVarible;
FOCCTRL            xdata   mcFocCtrl;
ILIMIT             xdata   mcIimit;
MCRAMP             xdata   mcSpeedRamp;
CONTROLCMDD        xdata   ConTrolCmd;
int16     Huan_temp = 0, Guan_temp = 0, Paiqi_temp = 0;
int16 qiangtujiaodu = 0;

#if (Debugg==1)
    extern int16  xdata GPFCON1, GPFCON2, GPFCON3, GPFCON4, GPFCON5, GPFCON6, GPFCON7, GPFCON8, GPFCON9, GPFCON10;
#endif

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);
    }
}

/*****转矩补偿和无转矩补偿时过流值设置*****/
void DCOvercurrentValue(void)
{
    static uint8 DAC_Value, DAC_Value2;
    DAC_Value = DAC_OvercurrentValue;
    DAC_Value2 = DAC0_DR;
    
    if (DAC_Value < DAC_Value2)
    {
        DAC_Value2 -= 1;
    }
    else if (DAC_Value > DAC_Value2)
    {
        DAC_Value2 += 1;
    }
    
    if (DAC_Value2 < 255)
    {
        DAC0_DR = (uint8)DAC_Value2;
    }
    else
    {
        DAC0_DR = 255;
    }
}
/*---------------------------------------------------------------------------*/
/*  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();
            DCOvercurrentValue();
            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 (mcFocCtrl.SpeedFlt > MOTOR_LOOP_RPM)
                {
                    if (Start_CNT < 15)
                    {
                        Start_CNT ++;
                    }
                    else
                    {
                        mcFocCtrl.CtrlMode = 1;
                        Start_CNT = 0;
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        mcSpeedRamp.ActualValue = mcFocCtrl.SpeedFlt + _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:
            {
                // IsLimit_Over_deal();
                mcFocCtrl.LoopTime++;
                mcFocCtrl.IND_DEC_LoopTime++;
                
                if (mcFocCtrl.IND_DEC_LoopTime > SPEED_INC_DEC_TIME)
                {
                    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 - mcFocCtrl.SpeedFlt);
                        #endif
                        
                        STT_FOC_THECOMP_CLEAR();
                    }
                    #else
                    {
                        mcFocCtrl.mcIqref = FOC_IQREF;
                    
                        if (FOC_IQREF < mcFocCtrl.QoutValue)
                        {
                            mcFocCtrl.mcIqref += QOUTINC;
                    
                            if (mcFocCtrl.mcIqref > mcFocCtrl.QoutValue)
                            { mcFocCtrl.mcIqref = mcFocCtrl.QoutValue; }
                    
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        else if (FOC_IQREF > mcFocCtrl.QoutValue)
                        {
                            mcFocCtrl.mcIqref -= QOUTINC;
                    
                            if (mcFocCtrl.mcIqref < mcFocCtrl.QoutValue)
                            { mcFocCtrl.mcIqref = mcFocCtrl.QoutValue; }
                    
                            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 (mcFocCtrl.SpeedFlt < _Q15(2000 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun;
        Ski = SKIRun;
        DQkp = DQKP;
        DQki = DQKI;
    }
    else if (mcFocCtrl.SpeedFlt < _Q15(4200 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun1;
        Ski = SKIRun1;
        DQkp = DQKP_zhong;
        DQki = DQKI_zhong;
    }
    else if (mcFocCtrl.SpeedFlt > _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;



void IsLimit_Over_tect(void)
{
    if (mcIimit.flag == 0 || mcIimit.flag == 3)
    {
        if ((mcFocCtrl.IDQFlt > I_OVER))
        {
            mcIimit.mcIDQtime3 = 0;
            
            if (mcIimit.mcIDQtime1 < 50)
            {
                mcIimit.mcIDQtime1++;
            }
            else
            {
                mcIimit.flag = 1;
            }
        }
    }
    
    if (mcIimit.flag == 1)
    {
        if ((mcFocCtrl.IDQFlt < I_LIMIT))
        {
            mcIimit.mcIDQtime1 = 0;
            
            if (mcIimit.mcIDQtime2 < 50)
            {
                mcIimit.mcIDQtime2++;
            }
            else
            {
                mcIimit.flag = 2;
            }
        }
        
        mcIimit.mcIDQtime4++;
        
        if (mcIimit.mcIDQtime4 >= 45000)
        {
            mcIimit.flag = 2;
            mcIimit.mcIDQtime4 = 0;
        }
    }
    
    if (mcIimit.flag == 2)
    {
        mcIimit.mcIDQtime2 = 0;
        
        if (mcIimit.mcIDQtime3 < 10000)
        {
            mcIimit.mcIDQtime3++;
        }
        else
        {
            mcIimit.flag = 3;
        }
    }
    
    //PPPPPP
    if (mcIimit.Pflag == 0 || mcIimit.Pflag == 3)
    {
        if ((mcFocCtrl.Powerlpf > P_OVER))
        {
            mcIimit.mcPtime3 = 0;
            
            if (mcIimit.mcPtime1 < 50)
            {
                mcIimit.mcPtime1++;
            }
            else
            {
                mcIimit.Pflag = 1;
            }
        }
    }
    
    if (mcIimit.Pflag == 1)
    {
        if ((mcFocCtrl.Powerlpf < P_LIMIT))
        {
            mcIimit.mcPtime1 = 0;
            
            if (mcIimit.mcPtime2 < 50)
            {
                mcIimit.mcPtime2++;
            }
            else
            {
                mcIimit.Pflag = 2;
            }
        }
        
        mcIimit.mcPtime4++;
        
        if (mcIimit.mcPtime4 >= 45000)
        {
            mcIimit.Pflag = 2;
            mcIimit.mcPtime4 = 0;
        }
    }
    
    if (mcIimit.Pflag == 2)
    {
        mcIimit.mcPtime2 = 0;
        
        if (mcIimit.mcPtime3 < 10000)
        {
            mcIimit.mcPtime3++;
        }
        else
        {
            mcIimit.Pflag = 3;
        }
    }
}

void IsLimit_Over_deal(void)
{
    if ((mcIimit.flag == 1) || mcIimit.Pflag == 1)
    {
        motorControl.TargetRef =  _Q15(1800 / MOTOR_SPEED_BASE);
    }
    else if ((mcIimit.flag == 2) || (mcIimit.Pflag == 2))
    {
        motorControl.TargetRef = mcSpeedRamp.ActualValue;
    }
}


uint16 VAC_Frequency_Max(void)
{
    static int16  Limit_Fre_Max = 0, VAC_last = 32765, VAC_chazhi = 0;
    VAC_chazhi = VAC_last - mcFocCtrl.mcAcbusFlt;
    
    if ((VAC_chazhi > _Q15(7.0 / HW_BOARD_ACVOLT_MAX)) || (VAC_chazhi < (_Q15(-7.0 / HW_BOARD_ACVOLT_MAX))) || (mcFocCtrl.mcAcbustime > 10000))
    {
        if (mcFocCtrl.mcAcbusFlt <= VAC_Min)
        {
            Limit_Fre_Max = VAC_Min_F;
        }
        else if (mcFocCtrl.mcAcbusFlt <= VAC_Max)
        {
            Limit_Fre_Max = VAC_Min_F + (int16)(VAC_Max_K * (mcFocCtrl.mcAcbusFlt - VAC_Min));
        }
        else
        {
            Limit_Fre_Max = VAC_Max_F ;
        }
        
        VAC_last = mcFocCtrl.mcAcbusFlt;
        mcFocCtrl.mcAcbustime = 0;
    }
    
    return Limit_Fre_Max;
}