/**
 * @copyright None
 * @file      MotorControl.c
 * @author    Comment Vivre
 * @date      2024-08-26
 * @brief     电机状态机切换
 */
#include <MyProject.h>

MotStateType data mcState;
MotStaM McStaSet;

/**
    @brief     电机控制状态机
    @warning   电机的状态只能在电机状态控制中切换，禁止在其他地方切换电机状态
    @date      2022-07-14
*/
void FiniteStateManger(void)
{
    // 故障下状态调整
    if (mcFaultSource != FaultNoSource)
    { mcState = mcFault;  }
    // 关机命令下状态调整
    else if ((mcState == mcInit) || (mcState == mcCharge)
        || (mcState == mcTailWind) || (mcState == mcPosiCheck)
        || (mcState == mcAlign) || (mcState == mcStart))
    {
        if (isCtrlPowOn == false)
        {  mcState = mcStop; }
    }
    
    // 状态机执行
    switch (mcState)
    {
        case mcReady:
        {
            Motor_Ready();
            
            if ((mcCurOffset.OffsetFlag == 1) && (isCtrlPowOn))
            {
                mcState = mcInit;
                mcCurOffset.OffsetFlag = 0;
                mcCurOffset.OffsetCount = 0; // 偏置电压采集计数
            }
            
            break;
        }
        
        case mcInit:
        {
            if (mcCurOffset.OffsetFlag == 1)
            {
                Motor_Init();
                #if (CHARGE_EN == Enable)
                {
                    mcFocCtrl.State_Count = CHARGE_TIME;
                    mcState = mcCharge; // 跳入mcCharge状态
                }
                #else
                {
                    #if (TAILWIND_MODE == NoTailWind)
                    {
                        #if (ALIGN_MOME != ALIGN_DSIABLE)
                        {
                            mcFocCtrl.mcPosCheckAngle = Align_Angle;
                            mcState = mcAlign;
                            mcFocCtrl.State_Count = Align_Time;
                        }
                        #else
                        mcState = mcStart;
                        #endif
                    }
                    #else
                    {
                        mcFocCtrl.State_Count = TAILWIND_TIME; // 顺逆风判断时间
                        mcState = mcTailWind;
                    }
                    #endif
                }
                #endif
            }
            
            break;
        }
        
        case mcCharge:
        {
            Motor_Charge();
            #if (IPMTEST == Enable)
            #else
            
            if (mcFocCtrl.State_Count == 0)
            {
                MOE = 0; // 关闭输出
                #if (TAILWIND_MODE == NoTailWind)
                {
                    #if (ALIGN_MOME != ALIGN_DSIABLE)
                    mcFocCtrl.mcPosCheckAngle = Align_Angle;
                    mcState = mcAlign;
                    mcFocCtrl.State_Count = Align_Time;
                    #else
                    mcState = mcStart;
                    #endif
                }
                #else
                mcFocCtrl.State_Count = TAILWIND_TIME; // 顺逆风判断时间
                mcState = mcTailWind;
                #endif
            }
            
            #endif
            break;
        }
        
        #if (TAILWIND_MODE != NoTailWind)
        
        case mcTailWind:
        {
            Motor_TailWind();
            break;
        }
        
        #endif
        #if (ALIGN_MOME != ALIGN_DSIABLE)
        
        case mcAlign:
        {
            Motor_Align();
            #if (ALIGN_MOME == ALIGN_TEST)
            
            while (1) ;
            
            #else
            {
                if (mcFocCtrl.State_Count == 0)
                { mcState = mcStart; }
            }
            #endif
            break;
        }
        
        #endif
        
        case mcStart:
        {
            if (mcFocCtrl.Start_Mode == TAILWIND_START) // 顺风启动
            {
                #if (TAILWIND_MODE == BEMFMethod)
                mcState = mcRun;
                #elif (TAILWIND_MODE == RSDMethod)
                mcState = mcRun;
                #elif (TAILWIND_MODE == FOCMethod)
                {
                    Motor_FocTailWind_Open();
                    mcState = mcRun;
                }
                #endif
            }
            else if (mcFocCtrl.Start_Mode == HEADWIND_START) // 逆风启动
            {
                if (mcFocCtrl.State_Count > 0) // 逆风启动刹车过程
                {
                    // 配置刹车代码
                    MC_Break();
                }
                else
                {
                    mcFocCtrl.State_Count = 200; // 顺逆风判断时间
                    mcState = mcReady;           // 刹车结束 切回顺逆风检测
                }
            }
            else // 静止启动
            {
                Motor_Static_Open();
                mcState = mcRun;
            }
            
            break;
        }
        
        case mcRun:
        {
            if (isCtrlPowOn == false)
            {
                mcState = mcStop;
                mcFocCtrl.State_Count = 5; // 单位：1ms
            }
            
            break;
        }
        
        case mcStop:
        {
            MC_Stop();
            break;
        }
        
        case mcBrake:
        {
            if (mcFocCtrl.State_Count == 0)
            {
                fault.Voltage.VoltDetecBraketDuty = 0;
                fault.Voltage.FlagBrakeInit = 0;
                MOE = 0;
                DRV_OUT = 0x00;
                ClrBit(DRV_CR, FOCEN);
                mcState = mcReady;
                mcFocCtrl.State_Count = 10;
            }
            
            break;
        }
        
        case mcFault:
        {
            // 欠压保护刹车处理函数
            if (mcFaultSource == FaultUnderVoltageDC)
            { UnderProcess(); }
            else
            {
                MOE = 0;
                ClrBit(DRV_CR, FOCEN); // 关闭FOC
                
                if (mcFaultSource == FaultNoSource)
                { mcState = mcReady; }
            }
            
            break;
        }
        
        default:
        {
            mcState = mcReady;
            break;
        }
    }
}

/**
    @brief        默认1ms周期服务函数，运行信号采样，调速信号处理，闭环控制，故障检测,ATO爬坡函数
                 该函数运行于大循环中，由SYSTICK定时器间隔1ms触发运行。
    @date         2022-07-14
*/

void TickCycle(void)
{
    #if (VoltageCompensationEn == 1)
    {
        if (VoltageComp.cpscnt <= VoltageCompensationDelayCnt)
        { SetBit(ADC_CR, ADCBSY);}
    }
    #else
    // 使能ADC的DCBUS采样
    SetBit(ADC_CR, ADCBSY);
    #endif
    
    if ((mcState != mcInit) && (mcState != mcReady))
    {
        // 速度滤波
        mcFocCtrl.SpeedFlt = LPFFunction(FOC__EOME, mcFocCtrl.SpeedFlt, 50); // 注意低通滤波器系数范围为0---127
        mcFocCtrl.EMFsquare = Sqrt_alpbet(FOC__EALP, FOC__EBET);
        mcFocCtrl.Is = Sqrt_alpbet(FOC__IA, FOC__IBET);
        
        if (mcState == mcRun)
        { mcFocCtrl.Power = LPFFunction(FOC__POW << 2, mcFocCtrl.Power, 50); }
        
        mcFocCtrl.UqFlt = LPFFunction(FOC__UQ, mcFocCtrl.UqFlt, 50);
        mcFocCtrl.UdFlt = LPFFunction(FOC__UD, mcFocCtrl.UdFlt, 50);
    }
    else
    {
        mcFocCtrl.SpeedFlt = 0;
        mcFocCtrl.EMFsquare = 0;
        mcFocCtrl.Is = 0;
        mcFocCtrl.UqFlt = 0;
        mcFocCtrl.Power = 0;
        mcFocCtrl.UdFlt = 0;
    }
    
    // DCbus的采样获取值并滤波
    #if (VoltageCompensationEn == 1)
    {
        // 未进入电压补偿之前在1ms中断里面采集电压，进入之后在载波中断里面采集电压
        if (VoltageComp.cpscnt <= VoltageCompensationDelayCnt)
        { mcFocCtrl.mcDcbusFlt = LPFFunction(ADC2_DR, mcFocCtrl.mcDcbusFlt, 50); }
        
        if ((mcFocCtrl.CtrlMode == 1) && (mcState == mcRun) && (mcFaultSource == FaultNoSource)) // 进入速度环后进行延时计数
        {
            if (VoltageComp.cpscnt < VoltageCompensationDelayCnt + 10)
            { VoltageComp.cpscnt++; }
            else
            { VoltageComp.cpscnt = VoltageCompensationDelayCnt + 10; }
        }
        else
        { VoltageComp.cpscnt = 0; }
        
        // 母线电压平均值
        BusAverageVoltage();
    }
    #else
    mcFocCtrl.mcDcbusFlt = LPFFunction(ADC2_DR, mcFocCtrl.mcDcbusFlt, 50); // 母线电压值滤波
    #endif
    mcFocCtrl.NTCValue = LPFFunction(ADC8_DR, mcFocCtrl.NTCValue, 100);
    mcFocCtrl.SREFValue = LPFFunction(ADC9_DR, mcFocCtrl.SREFValue, 100);
    // 获取调速信号，不同调速模式(PWMMODE,NONEMODE,SREFMODE)的目标值修改
    GetTargetRef();
    
    // 启动ATO控制，环路响应，如速度环、转矩环、功率环等
    if ((mcState == mcRun) || (mcState == mcStop))
    { LoopResponse(); }
    
    //故障保护函数功能，如过欠压保护、启动保护、缺相、堵转等
    Fault_Detection();
    
    // 电机启动ATO爬坡函数处理
    if (mcState == mcRun)
    { ATORamp(); }
    
    // 电机状态机的时序处理
    if (mcFocCtrl.State_Count > 0)
    { mcFocCtrl.State_Count--; }
}