#include <Myproject.h>

MotStaType   mcState;
MotStaM      McStaSet;

/**
    @function     Motor_Control_State
    @brief        电机控制状态机 负责状态跳转
    @date         2025-11-03
*/
void Motor_Control_State(void)
{
    // 故障直接跳转Fault
    if (mcFaultSource != FaultNoSource)
    { mcState = mcFault; }
    // 非Ready状态检测到关机信号跳转Stop
    else if ((isCtrlPowOn == false) && (mcState != mcReady))
    {
        // 若当前为Run 配置停机时间再跳转Stop
        if (mcState == mcRun)
        { mcFocCtrl.State_Count = 30000; }
        
        mcState = mcStop;
    }
    
    switch (mcState)
    {
        case mcReady:
        {
            if (McStaSet.SetFlag.CalibFlag == 0)
            {
                McStaSet.SetFlag.CalibFlag = 1;
                ClrBit(DRV_CR, FOCEN);
                MOE = 0;
                SetBit(ADC_MASK, CH1EN | CH0EN);
                mcCurOffset.OffsetCount = 0;
                mcCurOffset.OffsetFlag = 0;                  // 开始电流采集
            }
            
            if (mcCurOffset.OffsetFlag && isCtrlPowOn)
            {
                mcState = mcInit;
                ClrBit(DRV_CR, DRVEN);
                _nop_(); _nop_(); _nop_(); _nop_();
                SetBit(DRV_CR, DRVEN);
            }
            
            break;
        }
        
        case mcInit:
        {
            if ((mcFocCtrl.mcDcbusFlt > _Q15(Under_Protect_Voltage / HW_BOARD_VOLT_MAX)))
            {
                // 关闭软件电流采样的ADC  FOC模块会自动调用相应ADC 无需外部使能
                ClrBit(ADC_MASK, CH1EN | CH0EN);
                mcFaultSource = 0;
                memset(&mcFaultDect, 0, sizeof(FaultVarible));
                memset(&mcFocCtrl, 0, sizeof(FOCCTRL));
                memset(&mcIimit, 0, sizeof(ILIMIT));
                mcFocCtrl.mcDcbus_chazhi = 32760;
                McStaSet.SetMode = 0;
                memset(&VoltageComp, 0, sizeof(VOLCOMP));
                HW_Zero_PI_Init();
                HW_One_PI_Init();
                #if (FiledWeakenCompEnable)
                memset(&mcFieldWeaken, 0, sizeof(FieldWeakeningTypeDef));
                #endif
                mcState = mcAlign;
                mcFocCtrl.State_Count = Align_Time;
            }
            
            break;
        }
        
        case mcCharge: // 预充电状态，MCU输出固定频率占空比，预充电结束后，跳入mcTailWind
        {
            Motor_Charge();
            
            if (mcFocCtrl.State_Count == 0)
            {
                MOE = 0;                   // 关闭输出
                mcState = mcAlign;
                mcFocCtrl.State_Count     = Align_Time;
            }
            
            break;
        }
        
        case mcAlign:  // 预定位时间结束后，直接启动; AlignTestMode=1用于初始位置检测调试用
        {
            Motor_Align();
            #if (AlignTestMode)
            {
                while (1);
            }
            #endif
            break;
        }
        
        case mcStart:   // 配置电机启动参数，进入mcRun状态。
        {
            Motor_Open();
            break;
        }
        
        case mcRun:     // 运行状态，若运行状态的给定变为0，进入mcStop状态。
        {
            break;
        }
        
        case mcStop:
        {
            #if (StopBrakeFlag == 0)
            {
                mcState = mcReady;
                FOC_CR1 = 0x00;
                ClrBit(DRV_CR, FOCEN);   //关闭FOC
                MOE = 0;
            }
            #else
            
            if (motorControl.ActualSpeed < MOTOR_STOP_SPEED)
            {
                MOE = 0;
                FOC_CR1 = 0x00;
                ClrBit(DRV_CR, FOCEN);
                DRV_DR   = DRV_ARR + 1;
                DRV_CMR  = 0x00;
                DRV_CMR |= 0x015;     // 三相下桥臂通，刹车
                ClrBit(DRV_CR, OCS);  // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
                MOE = 1;
                mcState  = mcBrake;
                mcFocCtrl.State_Count = StopWaitTime;
            }
            
            #endif
            break;
        }
        
        case mcBrake:
        {
            if (isCtrlPowOn)
            {
                mcState = mcReady;
                mcFocCtrl.State_Count = 0;      //单位：1ms  强制关机时间
            }
            else if (mcFocCtrl.State_Count == 0)
            {
                mcState = mcReady;
                MOE = 0;
                ClrBit(DRV_CR, FOCEN);
            }
            
            break;
        }
        
        case mcFault:
        {
            if (mcFaultSource == FaultNoSource)
            { mcState = mcReady;}
            
            break;
        }
        
        default:
        {
            mcState = mcReady;
            break;
        }
    }
}


/**
    @function     Tick_Task
    @brief        周期任务
    @date         2025-11-03
*/
void Tick_Task(void)
{
    // 使能ADC的DCBUS采样 等待转换完成
    SetBit(ADC_CR, ADCBSY);
    
    while (ReadBit(ADC_CR, ADCBSY));
    
    if ((mcState == mcStart) || (mcState == mcRun) || (mcState == mcStop))
    {
        mcFocCtrl.BEmf         =  LPF_Zero_Update(FOC__EMF, mcFocCtrl.BEmf, LPF_K(5.0));
        mcFocCtrl.Powerlpf     =  LPF_Zero_Update(FOC__POW, mcFocCtrl.Powerlpf, LPF_K(1.0));
        mcFocCtrl.IDQFlt       =  LPF_Zero_Update(mcCurVarible.Max_is, mcFocCtrl.IDQFlt, LPF_K(1.0));
        mcFocCtrl.UDFlt       =  LPF_Zero_Update(FOC__UD, mcFocCtrl.UDFlt, LPF_K(30.0));          //LPF_K(30.0)
        mcFocCtrl.UQFlt       =  LPF_Zero_Update(FOC__UQ, mcFocCtrl.UQFlt, LPF_K(30.0));          //LPF_K(30.0)
        motorControl.ActualSpeed = LPF_Zero_Update(FOC__EOME, motorControl.ActualSpeed, LPF_K(71.0));
        
        if (mcFocCtrl.Powerlpf <= 0)
        {mcFocCtrl.Powerlpf = 0;}
    }
    else
    {
        mcFocCtrl.IDQFlt = 0;
        mcFocCtrl.BEmf = 0;
        mcFocCtrl.Powerlpf = 0;
        motorControl.ActualSpeed = 0;
    }
    
    #if 0
    User_Dri_timer1ms();     //user控制1ms中断
    IsLimit_Over_tect();     //限流处理
    #if (Debugg==1)
    {
        PC_MONITOR_1MS();   //工具
        CMDMONI_WT_SENDCMD();
    }
    #endif
    #endif
    StarRampDealwith();
}