AC-Fan-JingDa-FU6562T/User/Source/MotorControlFunction.c

/**
 * @copyright None
 * @file      MotorControlFunction.c
 * @author    Comment Vivre 
 * @date      2024-08-26
 * @brief     None
 */
#include <MyProject.h>

CurrentOffset xdata mcCurOffset; ///< 电流采样偏置电压采集缓存

/**
    @brief      对FOC的相关寄存器进行配置,先清理寄存器，后配置，最后使能
    @exception  初始化FOC，需要先关闭DRVIVER，对FOC寄存器配置完成后，使能FOC再打开DRVIVER,否则可能导致第一拍电流采样错误
    @date       2022-07-14
*/
void FOC_Init(void)
{
    DRV_CMR = 0x0ABF; // UH/VH/WH UL/VL/WL 互补并使能
    /* 使能FOC */
    ClrBit(DRV_CR, DRVEN); // 关闭DRVIVER 计时器,防止第一拍采样出错
    ClrBit(DRV_CR, FOCEN);
    SetBit(DRV_CR, FOCEN);
    ClrBit(FOC_CR0, MERRS1);
    SetBit(FOC_CR0, MERRS0);
    FOC_EOMEKLPF = _Q8(1.0); // FOC内部 速度滤波系数
    /* 配置FOC寄存器 */
    FOC_CR1 = 0;                     // 清零 FOC_CR1
    FOC_CR2 = 0;                     // 清零 FOC_CR2
    FOC_IDREF = 0;                   // 清零 Id
    FOC_IQREF = 0;                   // 清零 Iq
    FOC__THETA = 0;                  // 清零 角度
    FOC_RTHEACC = 0;                 // 清零 爬坡函数的初始加速度
    FOC__RTHESTEP = 0;               // 清零 爬坡速度
    FOC_RTHECNT = 0;                 // 清零 爬坡次数
    FOC_THECOMP = _Q15(-15.0 / 180.0); // SMO 估算补偿角
    FOC_THECOR = 0x04;               // 误差角度补偿
    /* 电流环参数配置 */
    FOC_DMAX = DOUTMAX;
    FOC_DMIN = DOUTMIN;
    FOC_QMAX = QOUTMAX;
    FOC_QMIN = QOUTMIN;
    /* 位置估算参数配置 */
    FOC_EK1 = OBS_K1T;
    FOC_EK2 = OBS_K2T;
    FOC_EK3 = OBS_K3T;
    FOC_EK4 = OBS_K4T;
    /* -----AO/PLL/SMO ----- */
    #if (EstimateAlgorithm == AO)
    {
        ClrBit(FOC_CR2, ESEL);
        FOC_KSLIDE = OBS_KSLIDE;
        FOC_EKLPFMIN = OBS_EA_KS;
        SetBit(FOC_CR0, ESCMS);
        SetBit(FOC_CR3, MFP_EN);
    }
    #elif (EstimateAlgorithm == SMO)
    {
        ClrBit(FOC_CR2, ESEL);
        FOC_KSLIDE = OBS_KSLIDE;
        FOC_EKLPFMIN = OBS_EA_KS;
    }
    #elif (EstimateAlgorithm == PLL)
    {
        SetBit(FOC_CR2, ESEL);
        FOC_KSLIDE = OBSE_PLLKP_GAIN;
        FOC_EKLPFMIN = OBSE_PLLKI_GAIN;
    }
    #else
    {
#error " EstimateAlgorithm  ERR"
    }
    #endif
    FOC_FBASE = OBS_FBASE;    // 由速度计算角度增量的系数
    FOC_OMEKLPF = SPEED_KLPF; // 估算器内速度低通滤波系数
    FOC_TGLI = PWM_TGLI_LOAD; // 死区配置
    SetBit(FOC_CR1, SVPWMEN); // SVPWM模式
    
    if (mcFocCtrl.FR == CW)
    {
        ClrBit(DRV_CR, DDIR); // 反转标志位
    }
    else
    {
        SetBit(DRV_CR, DDIR); // 反转标志位
    }
    
    /* 过调制 */
    #if (OverModulation == Enable)
    {
        SetBit(FOC_CR1, OVMDL); // 过调制
    }
    #endif // end OverModulation
    /* 单电阻采样； 需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式 */
    #if (Shunt_Resistor_Mode == Single_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
        FOC_TSMIN = PWM_TS_LOAD; // 最小采样窗口
        FOC_TRGDLY = 0x15;       // 采样时刻在中点，一般考虑开关噪声影响，会设置延迟；
        // 如：0x0c表示延迟12个clock，提前用反码形式，如0x84表示提前12个clock。
        ClrBit(FOC_CR2, F5SEG); // 7段式，单电阻仅支持7段式
    }
    /* 双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia Ib */
    #elif (Shunt_Resistor_Mode == Double_Resistor) // double resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
        FOC_TSMIN = PWM_DT_LOAD; // 死区补偿值
        FOC_TRGDLY = 0x07;       // ADC采样的时刻，采样时刻在计数器零点附近与单电阻不同
        // 如：0x83为下降沿结束前3个clock采样Ia 0x01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_DLOWL_TIME; // 下桥臂最小脉冲，保证采样
        /*五段式或七段式选择*/
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG); // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG); // 5段式
        }
        #endif
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS); // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS); // 5段式
        }
        #endif                                        // end DouRes_Sample_Mode
    }
    /*三电阻采样*/
    #elif (Shunt_Resistor_Mode == Three_Resistor) // signel resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0 | CSM1); // 三电阻
        FOC_TSMIN = PWM_DT_LOAD;                   // 死区补偿值
        FOC_TRGDLY = 0x06;                         // ADC采样的时刻，采样时刻在计数器零点附近与单电阻不同。
        // 如：0x83为下降沿结束前3个clock采样Ia，0x01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_OVERMODULE_TIME; // 过调制电流采样处理的TB脉宽
        /* 五段式或七段式选择 */
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG); // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG); // 5段式
        }
        #endif // end SVPMW_Mode
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS); // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS); // 5段式
        }
        #endif // end DouRes_Sample_Mode
    }
    #endif // end Shunt_Resistor_Mode
    /* 使能电流基准校正 */
    #if (CalibENDIS == Enable)
    {
        if (mcCurOffset.OffsetFlag == 1)
        {
            #if (Shunt_Resistor_Mode == Single_Resistor) // 单电阻校正
            {
                /*set ibus current sample offset*/
                SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                FOC_CSO = mcCurOffset.Iw_busOffset; // 写入Ibus的偏置
            }
            #elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻校正
            {
                /*set ia, ib current sample offset*/
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                FOC_CSO = mcCurOffset.IuOffset; // 写入IA的偏置
            
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                FOC_CSO = mcCurOffset.IvOffset; // 写入IB的偏置
            }
            #elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻校正
            {
                /*set ibus current sample offset*/
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                FOC_CSO = mcCurOffset.IuOffset; // 写入IA的偏置
            
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                FOC_CSO = mcCurOffset.IvOffset; // 写入IB的偏置
            
                SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                FOC_CSO = mcCurOffset.Iw_busOffset; // 写入IC的偏置
            }
            #endif                                         // end Shunt_Resistor_Mode
        }
    }
    #endif // end CalibENDIS
    /*  -------------------------------------------------------------------------------------------------
        DRV_CTL：PWM来源选择
        OCS = 0, DRV_COMR
        OCS = 1, FOC/SVPWM/SPWM
        -------------------------------------------------------------------------------------------------*/
    /*计数器比较值来源FOC*/
    SetBit(DRV_CR, DRVEN); // 计数器使能      0-->Disable 1-->Enable
    SetBit(DRV_CR, OCS);
}

/**
    @brief     预充电分三步，第一步是对U相进行预充电，第二步是对V两相进行预充电;第三步是对W三相进行预充电。
    @brief     低压应用一般不需要预充电，在customer.h禁止预充电功能寄即可
    @exception 在低功率高压应用中，过流值较小，充电电流过大可能导致触发硬件过流, 需要注意检查
    @date      2022-07-14
*/
void Motor_Charge(void)
{
    if (McStaSet.SetFlag.ChargeSetFlag == 0)
    {
        McStaSet.SetFlag.ChargeSetFlag = 1;
        SetBit(DRV_CR, DRVEN);                  // 计数器使能      0-->Disable 1-->Enable
        DRV_DR = (1.0 - CHARGE_DUTY) * DRV_ARR; // 下桥臂10% duty
        DRV_CMR &= 0x0000;
        /*  -------------------------------------------------------------------------------------------------
            DRV_CTL：PWM来源选择
            OCS = 0, DRV_COMR
            OCS = 1, FOC/SVPWM/SPWM
            -------------------------------------------------------------------------------------------------*/
        ClrBit(DRV_CR, OCS);
        mcFocCtrl.ChargeStep = 0;
    }
    
    if ((mcFocCtrl.State_Count < CHARGE_TIME) && (mcFocCtrl.ChargeStep == 0))
    {
        mcFocCtrl.ChargeStep = 1;
        DRV_CMR |= 0x01; // U相下桥臂通
        MOE = 1;
    }
    
    if ((mcFocCtrl.State_Count <= (CHARGE_TIME - 20)) && (mcFocCtrl.ChargeStep == 1))
    {
        mcFocCtrl.ChargeStep = 2;
        DRV_CMR |= 0x04; // V相下桥臂通
    }
    
    if ((mcFocCtrl.State_Count <= (CHARGE_TIME - 40)) && (mcFocCtrl.ChargeStep == 2))
    {
        mcFocCtrl.ChargeStep = 3;
        DRV_CMR |= 0x10; // W相下桥臂通
    }
    
    if ((mcFocCtrl.State_Count <= (CHARGE_TIME - 60)) && (mcFocCtrl.ChargeStep == 3))
    {
        mcFocCtrl.ChargeStep = 4;
        //        DRV_CMR &= 0x0000;
        //        DRV_CMR = 0x0ABF;
        //        DRV_DR = 0.975 * DRV_ARR;
    }
    
    if ((mcFocCtrl.State_Count <= (CHARGE_TIME - 75)) && (mcFocCtrl.ChargeStep == 4))
    {
        mcFocCtrl.ChargeStep = 5;
        DRV_CMR &= 0x0000;
        DRV_CMR = 0x003f;
    }
    
    if ((mcFocCtrl.State_Count <= (CHARGE_TIME - 76)) && (mcFocCtrl.ChargeStep == 5))
    {
        mcFocCtrl.State_Count = 0;
        mcFocCtrl.ChargeStep = 6;
    }
    
    #if (IPMTEST == Enable)
    {
        if (mcFocCtrl.ChargeStep == 6)
        {
            mcFocCtrl.ChargeStep = 7;
            DRV_CMR = 0x0ABF;
            DRV_DR = (1.0 - CHARGE_DUTY) * DRV_ARR; // 下桥臂10% duty
        }
    }
    #endif
}

/**
    @brief     开启次功能启动时候会将电机强拉到 设定角度，之后再启动
    @date      2022-07-14
*/
void Motor_Align(void)
{
    if (McStaSet.SetFlag.AlignSetFlag == 0)
    {
        McStaSet.SetFlag.AlignSetFlag = 1;
        /* -----FOC初始化----- */
        FOC_Init();
        /* 配置预定位的电流、KP、KI */
        FOC_IDREF = ID_Align_CURRENT;
        FOC_IQREF = IQ_Align_CURRENT;
        FOC_DKP = DQKP_Alignment;
        FOC_DKI = DQKI_Alignment;
        FOC_QKP = DQKP_Alignment;
        FOC_QKI = DQKI_Alignment;
        FOC_EKP = OBSW_KP_GAIN_START;
        FOC_EKI = OBSW_KI_GAIN_START;
        /*配置预定位角度*/
        FOC__THETA = Align_Theta;
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        {
            FOC__ETHETA = FOC__THETA - 4096;
        }
        #else
        {
            FOC__ETHETA = FOC__THETA;
        }
        #endif // end    EstimateAlgorithm
        /*使能输出*/
        //        DRV_CMR |= 0x3F;                         // U、V、W相输出
        MOE = 1;
    }
}

/**
    @brief     FOC计算方法顺逆风检测 的顺风启动配置函数
    @date      2022-07-14
*/
void Motor_FocTailWind_Open(void)
{
    /* 启动方式选择 */
    //    FOC_EFREQACC = 500;
    //    FOC_EFREQMIN = MOTOR_OMEGA_RAMP_MIN;
    //    FOC_EFREQHOLD = MOTOR_OMEGA_RAMP_END;
    FOC_QKP = QKP;
    FOC_QKI = QKI;
    FOC_DKP = DKP;
    FOC_DKI = DKI;
    mcFocCtrl.State_Count = 0;   // 取消ATO爬坡
    FOC_EKP = OBSW_KP_GAIN_RUN4; // 估算器里的PI的KP
    FOC_EKI = OBSW_KI_GAIN_RUN4; // 估算器里的PI的KI
    FOC_IQREF = IQ_RUN_CURRENT;
}

/**
    @brief     静止启动配置函数
    @date      2022-07-14
*/
void Motor_Static_Open(void)
{
    FOC_Init();
    MOE = 1;
    FOC__THETA = Align_Theta; // 无初始位置检测，则用预定位角
    /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
    FOC_IDREF = ID_Start_CURRENT;       // D轴启动电流
    mcFocCtrl.IqRef = IQ_Start_CURRENT; // Q轴启动电流
    FOC_DKP = DKP_Start;
    FOC_DKI = DKI_Start;
    FOC_QKP = QKP_Start;
    FOC_QKI = QKI_Start;
    FOC_EKP = OBSW_KP_GAIN_START;
    FOC_EKI = OBSW_KI_GAIN_START;
    /*启动方式选择*/
    #if (Open_Start_Mode == Omega_Start) // Omega 启动
    {
        FOC_EFREQACC = MOTOR_OMEGA_RAMP_ACC;
        FOC_EFREQMIN = MOTOR_OMEGA_RAMP_MIN;
        FOC_EFREQHOLD = MOTOR_OMEGA_RAMP_END;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
    }
    #elif (Open_Start_Mode == Open_Start)
    {
        FOC_RTHEACC = MOTOR_OPEN_RAMP_ACC;   // 爬坡函数的初始加速度
        FOC__RTHESTEP = MOTOR_OPEN_RAMP_MIN; // 0.62 degree acce speed
        FOC_RTHECNT = MOTOR_OPEN_RAMP_CNT;   // acce time
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
    }
    #elif (Open_Start_Mode == Open_Omega_Start)
    {
        FOC_RTHEACC = MOTOR_OPEN_RAMP_ACC;   // 爬坡函数的初始加速度
        FOC__RTHESTEP = MOTOR_OPEN_RAMP_MIN; // 0.62 degree acce speed
        FOC_RTHECNT = MOTOR_OPEN_RAMP_CNT;   // acce time
        FOC_EFREQACC = Motor_OMEGA_RAMP_ACC;
        FOC_EFREQMIN = MOTOR_OMEGA_RAMP_MIN;
        FOC_EFREQHOLD = MOTOR_OMEGA_RAMP_END;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | RFAE | ANGM);
    }
    #endif // end Open_Start_Mode
    /*不同启动方式下，切换到MCRUN状态*/
    #if (Open_Start_Mode == Open_Start) // OPEN状态启动时拖动多次
    {
        mcFocCtrl.State_Count = 0;
        FOC_EKP = OBSW_KP_GAIN_RUN4; // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN4; // 估算器里的PI的KI
    }
    #elif (Open_Start_Mode == Omega_Start)
    {
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO || EstimateAlgorithm == AO)
        {
            mcFocCtrl.State_Count = ATO_START_HOLDTIME + (ATO_RAMP_PERIOD << 2); // ATO 爬坡控制时间
        }
        #elif (EstimateAlgorithm == PLL)
        {
            mcFocCtrl.State_Count = 0;
            FOC_EKP = OBSW_KP_GAIN_RUN4; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4; // 估算器里的PI的KI
        }
        #endif                           // end    EstimateAlgorithm
    }
    #endif                           // end Open_Start_Mode
    FOC_IQREF = mcFocCtrl.IqRef; // Q轴启动电流
}

/**
    @brief     三下桥刹车
    @date      2022-07-14
*/
void MC_Break(void)
{
    MOE = 1;
    ClrBit(DRV_CR, FOCEN); // 关闭FOC
    /*  软件设置PWM占空比输出以上桥为参考，配置为互补输出时下桥反向 */
    ClrBit(DRV_CR, OCS);   // OCS = 0, DRV_COMR;  OCS = 1, FOC/SVPWM/SPWM
    SetBit(DRV_CR, DRVEN); // 计数器使能      0-->Disable 1-->Enable
    DRV_CMR = 0x0015;      //  关闭上桥输出，开启下桥输出
    DRV_DR = 0.95 * DRV_ARR;// ARR+4为全开
}


/**
    @brief     初始化电机参数，每次电机启动均会被调用
    @warning   需要注意填写的变量是否可以在此时刻被初始化
    @date      2022-07-14
*/
void VariablesPreInit(void)
{
    memset(&fault, 0, sizeof(FaultVarible)); // FaultVarible变量清零
    McStaSet.SetMode = 0;
    mcFocCtrl.CtrlMode = 0;
    LoopRefRamp.Out = 0;
    mcFocCtrl.ChargeStep = 0;
    mcFocCtrl.Flg_ATORampEnd = 0;
}

/**
    @brief     上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
              调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，OffsetFlag置1。
    @exception 默认循环1000次等待偏置电压稳定，需要注意若需要每次电机启动前都对偏置电压进行采样校准，
              那么需要保证偏置电压是稳定的，且采样计数变量已被清零（重新进行1000次循环)，否则可能导致采样出错
    @date      2022-07-14
*/
void GetCurrentOffset(void)
{
    SetBit(ADC_CR, ADCBSY); // 使能ADC
    
    while (ReadBit(ADC_CR, ADCBSY));
    
    #if (Shunt_Resistor_Mode == Single_Resistor) // 单电阻模式
    {
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
    }
    #elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
    {
        mcCurOffset.IuOffsetSum += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum -= mcCurOffset.IvOffset;
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
    }
    #elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
    {
        mcCurOffset.IuOffsetSum += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum -= mcCurOffset.IvOffset;
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
    }
    #endif
    mcCurOffset.OffsetCount++;
    
    if (mcCurOffset.OffsetCount > Calib_Time)
    {
        mcCurOffset.OffsetFlag = 1;
        #if (GetCurrentOffsetEnable)
        {
            Fault_GetCurrentOffset(); // 偏置电压保护
        }
        #endif
    }
}

/**
    @brief     关闭输出，关闭FOC，电机切换到mcReady状态被调用一次
    @date      2022-07-14
*/
void Motor_Ready(void)
{
    if (McStaSet.SetFlag.CalibFlag == 0)
    {
        McStaSet.SetFlag.CalibFlag = 1;
        MOE = 0;
        ClrBit(DRV_CR, FOCEN);
        ClrBit(DRV_CR, DRVEN);
        #if (Shunt_Resistor_Mode == Single_Resistor)
        {
            SetBit(ADC_MASK, CH4EN | CH2EN); // 开启外部ADC采集偏置电压
        }
        #else
        {
            SetBit(ADC_MASK, CH4EN | CH2EN | CH1EN | CH0EN); // 开启外部ADC采集偏置电压
        }
        #endif
    }
}

/**
    @brief     电机初始化，对电机相关变量、PI进行初始化设置,关闭FOC所需要使用到的ADC
    @note      关闭FOC所需要使用到的ADC，FOC模块会自动调用相应ADC 无需外部使能
    @date      2022-07-14
*/

void Motor_Init(void)
{
    #if (Shunt_Resistor_Mode == Single_Resistor)
    ClrBit(ADC_MASK, CH4EN); // 关闭软件电流采样的ADC  FOC模块会自动调用相应ADC 无需外部使能
    #else
    ClrBit(ADC_MASK, CH4EN | CH1EN | CH0EN); // 关闭软件电流采样的ADC  FOC模块会自动调用相应ADC 无需外部使能
    #endif
    VariablesPreInit(); // 电机相关变量初始化
    PI_Init();          // PI初始化
    // PI2_Init();                                    // PI初始化
    PI3_Init();         // PI初始化
}

/**
 * @brief     顺逆风处理函数
 * @date      2022-09-14
 */
void Motor_TailWind(void)
{
    if (McStaSet.SetFlag.TailWindSetFlag == 0) // 初始化
    {
        SetBit(DRV_CR, DRVEN); // 计数器使能      0-->Disable 1-->Enable
        McStaSet.SetFlag.TailWindSetFlag = 1;
        #if (TAILWIND_MODE == RSDMethod)
        RSDDetectInit();
        #elif (TAILWIND_MODE == BEMFMethod)
        BEMFDetectInit();
        #elif (TAILWIND_MODE == FOCMethod)
        FocDetectInit();
        #endif
    }
    
    #if (TAILWIND_MODE == RSDMethod)
    {
        if (mcFocCtrl.State_Count > 0) // 顺逆风检测时间结束前判断转速
        {
            mcState = RSDStartProcess();
        }
        else // 时间结束 还未检测到有效转速 则当做静止启动
        {
            ClrBit(CMP_CR2, CMP0EN); // 关闭比较器
            ClrBit(TIM2_CR1, T2CEN); // 0，停止计数；1,使能计数
            mcFocCtrl.Start_Mode = STATIC_START;
            #if (ALIGN_MOME != ALIGN_DSIABLE)
            mcFocCtrl.mcPosCheckAngle = Align_Angle;
            mcState = mcAlign;
            mcFocCtrl.State_Count = Align_Time;
            #else
            mcState = mcStart;
            #endif
        }
    }
    #elif (TAILWIND_MODE == BEMFMethod)
    {
        if (mcFocCtrl.State_Count > 0) // 顺逆风检测时间结束前判断转速
        {
            mcState = Bemf_Start_Process();
        }
        else // 时间结束 还未检测到有效转速 则当做静止启动
        {
            ClrBit(CMP_CR2, CMP0EN);  // 关闭比较器
            ClrBit(TIM1_CR0, T1BCEN); // 关闭定时器
            mcFocCtrl.Start_Mode = STATIC_START;
            #if (ALIGN_MOME != ALIGN_DSIABLE)
            mcFocCtrl.mcPosCheckAngle = Align_Angle;
            mcState = mcAlign;
            mcFocCtrl.State_Count = Align_Time;
            #else
            mcState = mcStart;
            #endif
        }
    }
    #elif (TAILWIND_MODE == FOCMethod)
    {
        if (mcFocCtrl.State_Count == 0) // 顺逆风检测时间结束判断转速
        {
            if ((mcFocCtrl.SpeedFlt >= S_Value(100)) && mcFocCtrl.EMFsquare >= 400) // 需要增加 判断保持时间
            {
                mcFocCtrl.Start_Mode = TAILWIND_START;
                mcState = mcStart;
            }
            else if ((mcFocCtrl.SpeedFlt < -S_Value(80)) && mcFocCtrl.EMFsquare >= 400)
            {
                McStaSet.SetFlag.TailWindSetFlag = 0;  // 清除 顺逆风检测初始标志
                mcFocCtrl.State_Count = 2000;          // 逆风刹车时间，刹车结束会切回重新进行顺逆风检测
                mcFocCtrl.Start_Mode = HEADWIND_START; // 逆风启动
                mcState = mcStart;                     // 状态机切换到Start
            }
            else
            {
                mcFocCtrl.Start_Mode = STATIC_START;
                mcState = mcStart;
            }
        }
    }
    #endif
}

/**
 * @brief     停止函数
 * @date      2022-09-14
 */
void MC_Stop(void)
{
    if ((mcFocCtrl.SpeedFlt < Stop_MOTOR_SPEED_RPM) || (mcFocCtrl.State_Count == 0)) // 实际转速低于Motor_Min_Speed或延时到了，关闭PWM输出或进入刹车
    {
        #if (StopBrakeFlag == 0)
        {
            FOC_CR1 = 0x00;
            /*关闭FOC*/
            ClrBit(DRV_CR, FOCEN);
            MOE = 0;
            mcState = mcBrake;
        }
        #else
        {
            MOE = 0;
            MC_Break();
            mcState = mcBrake;
            mcFocCtrl.State_Count = StopWaitTime;
        }
        #endif
    }
    else if (isCtrlPowOn) // Stop状态时，电机在减速状态，又开机进入正常运行模式
    {
        mcState = mcRun;
        mcFocCtrl.CtrlMode = 0;
        FOC_IQREF = IQ_RUN_CURRENT;
    }
}