#include <MyProject.h>

MotStateType  data mcState;
MotStaM     McStaSet;

/**
    @brief     电机控制状态机
    @warning   电机的状态只能在电机状态控制中切换，禁止在其他地方切换电机状态
    @date      2022-07-14
*/
void MC_Control(void)
{
    switch (mcState)
    {
        case mcReady:
            Motor_Ready();
            
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if ((mcCurOffset.OffsetFlag == 1) && isCtrlPowOn)
            { mcState = mcInit; }
            
            break;
            
        case mcInit:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (isCtrlPowOn == false)
            {
                mcState = mcStop;
            }
            else
            {
                Motor_Init();
                #if (CHARGE_EN == Enable)
                mcFocCtrl.State_Count   = CHARGE_TIME;
                mcState = mcCharge;
                #else
                #if (ALIGN_MOME != ALIGN_DSIABLE)
                mcFocCtrl.mcPosCheckAngle           = Align_Angle1;
                mcState                             = mcAlign;
                mcFocCtrl.State_Count               = AlignAll_Time;
                #else
                mcState = mcStart;
                #endif
                #endif
            }
            
            break;
            
        case mcCharge:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (isCtrlPowOn == false)
            {
                mcState = mcStop;
            }
            else
            {
                Motor_Charge();
                
                if (mcFocCtrl.State_Count == 0)
                {
                    MOE = 0;
                    #if (PosCheckEnable == Enable)
                    mcState                             = mcPosiCheck;
                    McStaSet.SetFlag.PosiCheckSetFlag   = 0;
                    mcFocCtrl.mcPosCheckAngle           = 0xffff;                     // 角度赋初值
                    #elif (ALIGN_MOME != ALIGN_DSIABLE)
                    mcFocCtrl.mcPosCheckAngle           = Align_Angle1;
                    mcState                             = mcAlign;
                    mcFocCtrl.State_Count               = AlignAll_Time;
                    #else
                    mcState = mcStart;
                    #endif
                }
            }
            
            break;
            #if (ALIGN_MOME != ALIGN_DSIABLE)
            
        case mcAlign:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (isCtrlPowOn == false)
            {
                mcState = mcStop;
            }
            else
            {
                Motor_Align();
                #if (ALIGN_MOME == ALIGN_TEST)
                
                while (1);
                
                #else
                
                if (mcFocCtrl.State_Count > (Align_Time1 + Align_Time2 + Align_Time3) )
                {
                    mcFocCtrl.CurrentAlignStatus = 0;
                    FOC__THETA = Align_Angle1;
                    FOC_IQREF  = IQ_Align_CURRENT * ((float)(AlignAll_Time - mcFocCtrl.State_Count) / AlignRamp_Time);
                }
                else if (mcFocCtrl.State_Count > (Align_Time2 + Align_Time3))
                {
                    mcFocCtrl.CurrentAlignStatus = 1;
                    FOC__THETA = Align_Angle2;
                    FOC_IQREF  = IQ_Align_CURRENT;
                }
                else if (mcFocCtrl.State_Count > (Align_Time3))
                {
                    mcFocCtrl.CurrentAlignStatus = 2;
                    mcFocCtrl.AngleProcess = Angle_AngleK * (Align_Time2 + Align_Time3 - mcFocCtrl.State_Count);
                    mcFocCtrl.AngleStart = Align_Angle2 + mcFocCtrl.AngleProcess;
                    FOC__THETA = mcFocCtrl.AngleStart;
                }
                else
                {
                    mcFocCtrl.CurrentAlignStatus = 2;
                    mcState = mcStart;
                }
                
                #endif
            }
            
            break;
            #endif
            
        case mcStart:
            Motor_Static_Open();
            mcState = mcRun;
            break;
            
        case mcRun:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (isCtrlPowOn == false)
            {
                mcState = mcStop;
                mcFocCtrl.State_Count = 3000;
            }
            
            break;
            
        case mcStop:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else
            {
                if ((mcFocCtrl.SpeedFlt < MOTOR_SPEED_STOP_RPM) && (mcFocCtrl.State_Count == 0))
                {
                    mcState = mcReady;
                    ClrBit(DRV_CR, FOCEN);
                    ClrBit(DRV_CR, DRVEN);
                    MOE = 0;
                }
            }
            
            break;
            
        case mcFault:
            if (mcFaultSource == FaultNoSource)
            {
                mcState = mcReady;
            }
            else
            {
                DRV_CMR &= 0xFFC0;
                ClrBit(DRV_CR, FOCEN);
                MOE = 0;
            }
            
            break;
    }
}