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:
        {
            switch (currOffset.OffsetCalib)
            {
                // 初始化状态
                case CALIB_INIT:
                {
                    if (powerControl.PowerSate == POWER_RUN)
                    {
                        ClrBit(DRV_CR, FOCEN);
                        MOE = 0;
                        SetBit(ADC_MASK, CH1EN | CH0EN);
                        currOffset.OffsetCount = 0;
                        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 = FaultIbusOffset;
                            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 ((mcFocCtrl.mcDcbusFlt > _Q15(Under_Protect_Voltage / HW_BOARD_VOLT_MAX)))
            {
                // 关闭软件电流采样的ADC
                mcFaultSource = 0;
                memset(&mcFaultDect, 0, sizeof(FaultVarible));
                memset(&mcFocCtrl, 0, sizeof(FOCCTRL));
                mcFocCtrl.mcDcbus_chazhi = 32760;
                McStaSet.SetMode = 0;
                memset(&VoltageComp, 0, sizeof(VOLCOMP));
                // PI控制器初始化
                HW_Zero_PI_Init();
                HW_One_PI_Init();
                #if (FiledWeakenCompEnable)
                memset(&mcFieldWeaken, 0, sizeof(FieldWeakeningTypeDef));
                #endif
                // 此模块不需要预充电
                #if 0
                mcState = mcCharge;
                mcFocCtrl.ChargeStep = 0;
                mcFocCtrl.State_Count = 0;
                #else
                mcState = mcAlign;
                mcFocCtrl.State_Count = Align_Time;
                #endif
            }
            break;
        }
        
        case mcCharge:
        {
            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:
                {
                    #if (1)
                    DRV_CMR |= 0x3F;
                    #else
                    MOE = 0;
                    mcState = mcAlign;
                    mcFocCtrl.State_Count = Align_Time;
                    #endif
                    break;
                }
            }
            
            if ((mcFocCtrl.State_Count == 0) && (mcFocCtrl.ChargeStep != 4 ))
            {
                mcFocCtrl.ChargeStep ++;
                mcFocCtrl.State_Count = 30;
            }
            
            break;
        }
        
        case mcAlign:
        {
            Motor_Align();
            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();
}