AC-Motor-FU6815/User/Function/FocControl.c

#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));
                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 = mcCharge;
            }
            
            break;
        }
        
        case mcCharge:
        {
            if ((mcFocCtrl.State_Count == 0) && (mcFocCtrl.ChargeStep !=5 ))
            {
                    mcFocCtrl.ChargeStep ++;
                    mcFocCtrl.State_Count = 30;
                #if (1)
            
                if (mcFocCtrl.ChargeStep == 4)
                { mcFocCtrl.ChargeStep = 5; }
                
                #endif
            }
            
            switch (mcFocCtrl.ChargeStep)
            {
                case 0:
                {
                    // 配置占空比
                    DRV_DR = Calib_Duty * DRV_ARR;
                    // DRV_CTL：PWM来源选择
                    // OCS = 0, DRV_COMR
                    // OCS = 1, FOC/SVPWM/SPWM
                    ClrBit(DRV_CR, OCS);
                    mcFocCtrl.ChargeStep = 1;
                    mcFocCtrl.State_Count = 30;
                    break;
                }
                
                // U相下桥臂通
                case 1:
                {
                    DRV_CMR |= 0x01;
                    MOE = 1;
                    break;
                }
                
                // V相下桥臂导通
                case 2:
                {
                    DRV_CMR |= 0x04;
                    break;
                }
                
                // W相下桥臂导通
                case 3:
                {
                    DRV_CMR |= 0x10;
                    break;
                }
                
                case 4:
                {
                    MOE = 0;
                    mcState = mcCharge;
                    mcFocCtrl.State_Count = Align_Time;
                    break;
                }
                
                case 5:
                {
                    DRV_CMR |= 0x3F;
                    break;
                }
            }
            
            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)
{
    if ((mcState == mcStart) || (mcState == mcRun) || (mcState == mcStop))
    {
        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));
        motorControl.BackEMF = LPF_Zero_Update(FOC__EMF, motorControl.BackEMF, LPF_K(5.0));
        
        if (mcFocCtrl.Powerlpf <= 0)
        {mcFocCtrl.Powerlpf = 0;}
    }
    else
    {
        mcFocCtrl.IDQFlt = 0;
        mcFocCtrl.Powerlpf = 0;
        motorControl.ActualSpeed = 0;
        motorControl.BackEMF = 0;
    }
    
    StarRampDealwith();
}