#include <MyProject.h>


bool  isCtrlPowOn = false;                  ///< 开关机控制

FOCCTRL xdata mcFocCtrl;
MCRAMP xdata mcRefRamp;
int16 refRampOut = 0;

/**
    @brief        对变量取16位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint16 Abs_F16(int16 value)
{
    if (value < 1)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
    @brief        对变量取32位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint32 Abs_F32(int32 value)
{
    if (value < 1)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}



/**
    @brief        调速信号处理包含：开关机控制、将调速信号处理成控制目标给定信号
    @date         2022-07-14
*/
void TargetRef_Process(void)
{
    #if (SPEED_MODE == NONEMODE)
	isCtrlPowOn = true;  // 开机
    mcFocCtrl.Ref = MOTOR_SPEED_MAX_RPM;
    #elif (SPEED_MODE == PWMMODE)
    #endif
}

/**
    @brief        外部闭环控制函数，示例代码提供 电流环，速度环，功率环，UQ控制示例代码,可根据需要自行修改
                 建议使用默认1ms周期运行
    @date         2022-07-14
*/
void Speed_response(void)
{
    uint16 FOC_KSLIDE_Temp = 0;
    uint16 FOC_EKLPFMIN_Temp = 0;
    
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        switch (mcFocCtrl.CtrlMode)
        {
            case 0:
            {
                if (mcFocCtrl.SpeedFlt > MOTOR_LOOP_RPM)
                {
                    mcFocCtrl.Mode0HoldCnt++;
                    
                    if (mcFocCtrl.Mode0HoldCnt > 5)
                    {
                        mcFocCtrl.CtrlMode  = 1;
                        FOC_QKP             = QKP;
                        FOC_QKI             = QKI;
                        FOC_DKP             = DKP;
                        FOC_DKI             = DKI;
                        // ClrBit(FOC_CR2,UDD);
                        mcRefRamp.OutValue_float = S_Value(2000.0);
                        mcFocCtrl.LoopTime = LOOP_TIME;
                        mcFocCtrl.IqRef = FOC_IQREF;
                        FOC_IDREF = 0;
                        PI1_UKH = IQ_Start_CURRENT;
                    }
                }
                else
                {
                    mcFocCtrl.Mode0HoldCnt = 0;
                }
            }
            break;
            
            case 1:
            {
                mcFocCtrl.LoopTime++;
                
                if (mcFocCtrl.LoopTime >= LOOP_TIME)
                {
                    mcFocCtrl.LoopTime = 0;
                    refRampOut = Motor_Ramp(mcFocCtrl.Ref);
                    FOC_IQREF = HW_Zero_Calc(refRampOut - mcFocCtrl.SpeedFlt);
                    FOC_IDREF = 0;
                }
            }
            break;
        }
    }
}



/**
    @brief        控制给定爬坡函数
                 以浮点进行计算，解决整数爬坡由于精度的影响，导致爬坡结果阶梯变化
                 函数控制周期默认为闭环控制周期，建议使用默认1ms周期运行
    @param[in]    ref 给定目标值
    @return       爬坡结果（int16）
    @date         2022-07-14
*/
int16 Motor_Ramp(int16 ref)
{
    mcRefRamp.RefValue = ref;
    
    if (mcRefRamp.OutValue_float < mcRefRamp.RefValue)
    {
        if (mcRefRamp.OutValue_float + mcRefRamp.IncValue < mcRefRamp.RefValue)
        {
            mcRefRamp.OutValue_float += mcRefRamp.IncValue;
        }
        else
        {
            mcRefRamp.OutValue_float = mcRefRamp.RefValue;
        }
    }
    else if (mcRefRamp.OutValue_float > mcRefRamp.RefValue)
    {
        if (mcRefRamp.OutValue_float - mcRefRamp.DecValue > mcRefRamp.RefValue)
        {
            mcRefRamp.OutValue_float -= mcRefRamp.DecValue;
        }
        else
        {
            mcRefRamp.OutValue_float = mcRefRamp.RefValue;
        }
    }
    else
    {
        mcRefRamp.OutValue_float = mcRefRamp.RefValue;
    }
    
    return (int16)mcRefRamp.OutValue_float;                                                         // 输出浮点数取整
}


/**
    @brief        启动ATO爬坡函数，用于静止启动时候对ATO进行爬坡，提高启动可靠性
    @date         2022-07-14
*/
void ATORamp(void)
{
    if (mcState == mcRun)
    {
        if (mcFocCtrl.State_Count == (ATO_RAMP_PERIOD << 2))
        {
            FOC_EKP = OBSW_KP_GAIN_RUN1;              // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1;              // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == ((ATO_RAMP_PERIOD << 1) + ATO_RAMP_PERIOD))
        {
            FOC_EKP = OBSW_KP_GAIN_RUN2;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2;             // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == (ATO_RAMP_PERIOD << 1))
        {
            FOC_EKP = OBSW_KP_GAIN_RUN3;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN3;             // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count <= ATO_RAMP_PERIOD && mcFocCtrl.Flg_ATORampEnd == 0)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN4;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4;             // 估算器里的PI的KI
            mcFocCtrl.Flg_ATORampEnd = 1;            // ATO 爬坡结束
        }
    }
}
void TickCycle_1ms(void)
{
    
}