#include <Myproject.h>

MotStaType   mcState;

/**
    @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:
            switch (currOffset.OffsetCalib)
            {
                // 初始化状态
                case CALIB_INIT:
                    if (powerControl.PowerSate == POWER_RUN)
                    {
                        ClrBit(DRV_CR, FOCEN);
                        MOE = 0;
                        // 使能ADC转换通道
                        SetBit(ADC_MASK, CH1EN | CH0EN);
                        // 电流校准变量初始化
                        memset(&currOffset, 0, sizeof(Curr_Offset_t));
                        // 配置初始值
                        currOffset.IuOffsetSum = 16383;
                        currOffset.IvOffsetSum = 16383;
                        currOffset.IwBusOffsetSum = 16383;
                        // 跳转状态
                        currOffset.OffsetCalib = GET_OFFSET;
                    }
                    
                    break;
                    
                // 检查状态
                case GET_OFFSET:
                    SetBit(ADC_CR, ADCBSY);
                    
                    while (ReadBit(ADC_CR, ADCBSY));
                    
                    currOffset.IuOffsetSum += ((ADC0_DR & 0x7ff8));
                    currOffset.IuOffset = currOffset.IuOffsetSum >> 4;
                    currOffset.IuOffsetSum -= currOffset.IuOffset;
                    currOffset.IvOffsetSum += ((ADC1_DR & 0x7ff8));
                    currOffset.IvOffset = currOffset.IvOffsetSum >> 4;
                    currOffset.IvOffsetSum -= currOffset.IvOffset;
                    currOffset.IwBusOffset = currOffset.IvOffset;
                    
                    if (++currOffset.OffsetCount > 1000)
                    {
                        //进入偏置电压错误保护
                        if (((currOffset.IuOffset > 19959) || (currOffset.IuOffset < 10107)) ||
                            ((currOffset.IvOffset > 19959) || (currOffset.IvOffset < 10107)))
                        {
                            currOffset.OffsetCalib = CALIB_INIT;
                            mcFaultSource = FaultOffsetError;
                            FaultProcess();
                        }
                        else
                        { currOffset.OffsetCalib = OFFSET_READY; }
                    }
                    
                    break;
                    
                // 启动状态
                case OFFSET_READY:
                    if (isCtrlPowOn)
                    {
                        mcState = mcInit;
                        // 关闭软件电流采样的ADC
                        ClrBit(ADC_MASK, CH1EN | CH0EN);
                        currOffset.OffsetCalib = CALIB_INIT;
                        ClrBit(DRV_CR, DRVEN);
                        _nop_(); _nop_(); _nop_(); _nop_();
                        SetBit(DRV_CR, DRVEN);
                    }
                    
                    break;
            }
            
            break;
            
        case mcInit:
            if (motorControl.DCBus > UNDER_RECOVER_VALUE)
            {
                mcFaultSource = 0;
                memset(&mcFaultDect, 0, sizeof(FaultVarible));
                memset(&mcFocCtrl, 0, sizeof(FOCCTRL));
                memset(&motorControl, 0, sizeof(Motor_Control_t));
                // PI控制器初始化
                HW_Zero_PI_Init();
                // 弱磁初始化
                #if (FiledWeakenCompEnable)
                memset(&mcFieldWeaken, 0, sizeof(FieldWeakeningTypeDef));
                HW_One_PI_Init();
                #endif
                // 环路配置
                #if (SPEED_CONTROL_MODE)
                motorControl.RampInc = MOTOR_SPEED_INC;
                motorControl.RampDec = MOTOR_SPEED_DEC;
                #endif
                // 此模块不需要预充电
                #if (PRE_DRIVER_CHARGE)
                mcState = mcCharge;
                motorControl.ChargeState = CHARGE_INIT;
                #else
                mcState = mcAlign;
                #endif
            }
            
            break;
            #if (PRE_DRIVER_CHARGE)
            
        case mcCharge:
            switch (motorControl.ChargeState)
            {
                case CHARGE_INIT:
                    // 配置占空比
                    DRV_DR = CALIB_DUTY * DRV_ARR;
                    // DRV_CTL：PWM来源选择
                    // OCS = 0, DRV_COMR
                    // OCS = 1, FOC/SVPWM/SPWM
                    ClrBit(DRV_CR, OCS);
                    motorControl.ChargeState = U_BRIDGE;
                    mcFocCtrl.State_Count = 30;
                    break;
                    
                // U相下桥臂通
                case U_BRIDGE:
                    DRV_CMR |= 0x01;
                    MOE = 1;
                    
                    if (!mcFocCtrl.State_Count)
                    {
                        mcFocCtrl.State_Count = 30;
                        motorControl.ChargeState = V_BRIDGE
                    }
                    
                    break;
                    
                // V相下桥臂导通
                case V_BRIDGE:
                    DRV_CMR |= 0x04;
                    
                    if (!mcFocCtrl.State_Count)
                    {
                        mcFocCtrl.State_Count = 30;
                        motorControl.ChargeState = W_BRIDGE
                    }
                    
                    break;
                    
                // W相下桥臂导通
                case W_BRIDGE:
                    DRV_CMR |= 0x10;
                    
                    if (!mcFocCtrl.State_Count)
                    { motorControl.ChargeState = CHARGE_END; }
                    
                    break;
                    
                // 启动
                case CHARGE_END:
                    #if (0)
                    DRV_CMR |= 0x3F;
                    #else
                    MOE = 0;
                    mcState = mcAlign;
                    #endif
                    break;
            }
            
            break;
            #endif
            
        case mcAlign:
            switch (motorControl.AlignState)
            {
                // 预定位初始化
                case ALIGN_INIT:
                    // FOC初始化
                    FOC_Init();
                    // 配置预定位的电流、KP、KI
                    FOC_IDREF = 0;
                    FOC_IQREF = 0;
                    FOC_DKP   = DQKP_Alignment;
                    FOC_DKI   = DQKI_Alignment;
                    FOC_QKP   = DQKP_Alignment;
                    FOC_QKI   = DQKI_Alignment;
                    FOC_EKP   = OBSW_KP_GAIN;
                    FOC_EKI   = OBSW_KI_GAIN;
                    // 配置预定位角度
                    #if (EstimateAlgorithm == SMO)
                    FOC__ETHETA   = FOC__THETA - 4836;
                    #elif (EstimateAlgorithm == PLL)
                    FOC__ETHETA   = FOC__THETA;
                    #endif
                    DRV_CMR |= 0x03F;
                    MOE = 1;
                    mcFocCtrl.State_Count = 500;
                    motorControl.AlignState = ALIGN_CURR_RAMP;
                    break;
                    
                // 第一角度预定位电流爬坡
                case ALIGN_CURR_RAMP:
                    FOC__THETA = ALIGN_ANGLE_1;
                    FOC_IQREF = IQ_ALIGN_CURRENT * (AlignmentRampTime - mcFocCtrl.State_Count) / AlignmentRampTime;
                    
                    if (!mcFocCtrl.State_Count)
                    {
                        mcFocCtrl.State_Count = 500;
                        motorControl.AlignState = ALIGN_FIRST_ANGLE;
                    }
                    
                    break;
                    
                // 第一角度预定位
                case ALIGN_FIRST_ANGLE:
                    FOC_IQREF = IQ_ALIGN_CURRENT;
                    FOC__THETA = ALIGN_ANGLE_1;
                    
                    if (!mcFocCtrl.State_Count)
                    {
                        mcFocCtrl.State_Count = 500;
                        motorControl.AlignState = ALIGN_SECOND_ANGLE;
                    }
                    
                    break;
                    
                // 第二角度预定位
                case ALIGN_SECOND_ANGLE:
                    FOC_IQREF = IQ_ALIGN_CURRENT;
                    FOC__THETA = ALIGN_ANGLE_2;
                    
                    if (!mcFocCtrl.State_Count)
                    {
                        mcFocCtrl.State_Count = 500;
                        motorControl.AlignState = ALIGN_INIT;
                        mcState = mcStart;
                    }
                    
                    break;
            }
            
            break;
            
        case mcStart:
            Motor_Open();
            break;
            
        case mcRun:
            break;
            
        case mcStop:
            #if (STOP_BREAK)
            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 = BREAK_WAIT_TIME;
            }
            
            #else
            mcState = mcReady;
            FOC_CR1 = 0x00;
            ClrBit(DRV_CR, FOCEN);   //关闭FOC
            MOE = 0;
            #endif
            break;
            #if (STOP_BREAK)
            
        case mcBrake:
            if (isCtrlPowOn)
            {
                mcState = mcReady;
                mcFocCtrl.State_Count = 0;
            }
            else if (mcFocCtrl.State_Count == 0)
            {
                mcState = mcReady;
                MOE = 0;
                ClrBit(DRV_CR, FOCEN);
            }
            
            break;
            #endif
            
        case mcFault:
            if (mcFaultSource == FaultNoSource)
            { mcState = mcReady;}
            
            break;
            
        default:
            mcState = mcReady;
            break;
    }
}


/**
    @function     Tick_Task
    @brief        周期任务
    @date         2025-11-03
*/
void Tick_Task(void)
{
    if ((mcState == mcStart) || (mcState == mcRun) || (mcState == mcStop))
    {
        mcFocCtrl.Powerlpf =  LPF_Zero_Update(FOC__POW, mcFocCtrl.Powerlpf, LPF_K(1.0));
        mcFocCtrl.UDFlt =  LPF_Zero_Update(FOC__UD, mcFocCtrl.UDFlt, LPF_K(30.0));
        mcFocCtrl.UQFlt =  LPF_Zero_Update(FOC__UQ, mcFocCtrl.UQFlt, LPF_K(30.0));
        motorControl.ActualSpeed = LPF_Zero_Update(FOC__EOME, motorControl.ActualSpeed, LPF_K(71.0));
        motorControl.BackEMF = LPF_Zero_Update(FOC__EMF, motorControl.BackEMF, LPF_K(5.0));
        
        if (mcFocCtrl.Powerlpf <= 0)
        {mcFocCtrl.Powerlpf = 0;}
    }
    else
    {
        mcFocCtrl.Powerlpf = 0;
        motorControl.ActualSpeed = 0;
        motorControl.BackEMF = 0;
    }
}