AC-Motor-FU6815/User/Application/FocControlFunction.c

/********************************************************************************

 **** Copyright (C), 2020, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
    File Name     : FocControlFunction.c
    Author        : Bruce HW&RD
    Date          : 2020-10-13
    Description   : .C file function description
    Version       : 1.0
    Function List :

    Record        :
    1.Date        : 2020-10-13
     Author      : Bruce HW&RD
     Modification: Created file

********************************************************************************/

#include "FU68xx_5.h"
#include <Myproject.h>



CurrentOffset xdata mcCurOffset;
/*---------------------------------------------------------------------------*/
/*  Name     :   void FOC_Init(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: mcInit状态下，对FOC的相关寄存器进行配置,先清理寄存器，后配置，最后使能
    /*---------------------------------------------------------------------------*/
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);
    SetBit(FOC_CR0, MERRS1);  //FTC6804新增200190530
    SetBit(FOC_CR0, MERRS0);  //FTC6804新增200190530
    FOC_EOMEKLPF    = 0xff;   //速度滤波系数,值越小滤波深度越深
    FOC_KFG         = 0;   //FG计算系数 = 6M(TIM4分频结果)/640（FBase）
    /*配置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     = Start_FOC_THECOMP;                      // SMO 估算补偿角
    FOC_THECOR      = 0x02;                                 // 误差角度补偿
    mcFocCtrl.STT_FOC_THECOMP = Start_FOC_THECOMP;
    /*电流环参数配置*/
    FOC_DKP = DQKP;
    FOC_DKI = DQKI;
    FOC_QKP = DQKP;
    FOC_QKI = DQKI;
    FOC_ID_LPFK = 250;
    FOC_IQ_LPFK = 250;
    FOC_DMAX        = DOUTMAX;
    FOC_DMIN        = DOUTMIN;
    FOC_QMAX        = QOUTMAX;
    FOC_QMIN        = QOUTMIN;
    SetBit(FOC_CR0, ESCMS);   //选择arctan估算模式
    /*位置估算参数配置*/
    /*********PLL或SMO**********/
    #if (EstimateAlgorithm == SMO)
    {
        ClrBit(FOC_CR2, ESEL);
        ClrBit(FOC_CR3, MFP_EN);
    }
    #elif (EstimateAlgorithm == AO)
    {
        SetBit(FOC_CR3, MFP_EN);
        ClrBit(FOC_CR2, ESEL);
    }
    #elif (EstimateAlgorithm == PLL)
    {
        ClrBit(FOC_CR3, MFP_EN);
        SetBit(FOC_CR2, ESEL);
        FOC_KSLIDE    = OBSE_PLLKP_GAIN1;
        FOC_EKLPFMIN  = OBSE_PLLKI_GAIN1;
    }
    #endif //end SVPMW_Mode
    FOC_EK1         = OBS_K1T;
    FOC_EK2         = OBS_K2T;
    FOC_EK3         = OBS_K3T;
    FOC_EK4         = OBS_K4T;
    FOC_KSLIDE    = OBS_KSLIDE;
    FOC_EKLPFMIN  = OBS_EA_KS;
    FOC_FBASE       = OBS_FBASE;
    FOC_OMEKLPF     = SPEED_KLPF;
    FOC_TGLI        = PWM_TGLI_LOAD;
    SetBit(FOC_CR1, SVPWMEN);                            // SVPWM模式
    #if (IRMODE == 1)
    {
        SetBit(DRV_CR, DDIR);                            // 反转标志位
    }
    #elif (IRMODE == 0)
    {
        ClrBit(DRV_CR, DDIR);                            // 反转标志位
    }
    #endif //end IRMODE
    /**过调制**/
    #if (OverModulation == 1)
    {
        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 = 0x3B;                                  // 采样时刻在中点，一般考虑开关噪声影响，会设置延迟；3B
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                  // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                  // 5段式
        }
        #endif
    }
    /*双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia，配置81*/
    #elif (Shunt_Resistor_Mode == Double_Resistor) // double resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
        FOC_TSMIN = PWM_DT_LOAD;                   // 死区补偿值
        FOC_TRGDLY = 0x05;                         // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同
        // 01为上升沿开始后第一个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 = 0x08;        // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同。
        // 01为上升沿开始后第一个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
        -------------------------------------------------------------------------------------------------*/
    //    ClrBit(DRV_CR , DRVEN);
    //      _nop_();_nop_();_nop_();_nop_();
    SetBit(DRV_CR, DRVEN);
    /*计数器比较值来源FOC*/
    SetBit(DRV_CR, OCS);
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Charge(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 预充电，当一直处于预充电状态下，不接电机，可用于验证IPM或者Mos。
    预充电分三步
    第一步是对U相进行预充电
    第二步是对U,V两相进行预充电
    第三步是对U、V、W三相进行预充电。
    /*---------------------------------------------------------------------------*/
void Motor_Charge(void)
{
    if (McStaSet.SetFlag.ChargeSetFlag == 0)
    {
        McStaSet.SetFlag.ChargeSetFlag = 1;
        DRV_DR = Calib_Duty * DRV_ARR;              //下桥臂10% duty
        //          DRV_CMR = 0x00;
        /*  ---------------------------------------------------------------------
            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 << 1) / 3) && (mcFocCtrl.ChargeStep == 1))
    {
        mcFocCtrl.ChargeStep = 2;
        DRV_CMR |= 0x04;                    // V相下桥臂导通
    }
    
    if ((mcFocCtrl.State_Count <= Charge_Time / 3) && (mcFocCtrl.ChargeStep == 2))
    {
        mcFocCtrl.ChargeStep = 3;
        DRV_CMR |= 0x10;                    // W相下桥臂导通
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Align(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 预定位函数，当无逆风判断时，采用预定位固定初始位置;当有逆风判断时，采用预定位刹车
    /*---------------------------------------------------------------------------*/
void Motor_Align(void)
{
    if (McStaSet.SetFlag.AlignSetFlag == 0)
    {
        McStaSet.SetFlag.AlignSetFlag = 1;
        /*FOC初始化*/
        FOC_Init();
        /*配置预定位的电流、KP、KI*/
        FOC_IDREF = 0;//ID_Align_CURRENT;
        FOC_IQREF = 0;//IQ_Align_CURRENT;
        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;
        //        /*配置预定位角度*/
        //        FOC__THETA  = 0;//Align_Theta;
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        FOC__ETHETA   = FOC__THETA - 4836;
        #elif (EstimateAlgorithm == PLL)
        FOC__ETHETA   = FOC__THETA;
        #endif //end    EstimateAlgorithm
        /*使能输出*/
        DRV_CMR |= 0x0abF;      // U、V、W相上下互补输出 FTC6805
        MOE = 1;
    }
    
    if (mcFocCtrl.State_Count > (AlignmentHoldTime1 + AlignmentHoldTime2)) /* Ramp 1s */
    {
        mcFocCtrl.CurrentAlignStatus = 0;
        FOC__THETA      = Align_Angle1;
        FOC_IDREF       = ID_Align_CURRENT_End * (Align_Time - mcFocCtrl.State_Count) / AlignmentRampTime;
    }
    else if (mcFocCtrl.State_Count > AlignmentHoldTime2)             /* Hold 900ms*/
    {
        mcFocCtrl.CurrentAlignStatus = 1;
        FOC__THETA      = Align_Angle1;
        FOC_IDREF       = ID_Align_CURRENT_End;
    }
    else if (mcFocCtrl.State_Count > 0)             /* Hold */
    {
        mcFocCtrl.CurrentAlignStatus = 2;
        FOC__THETA      = Align_Angle2;
        FOC_IDREF       = ID_Align_CURRENT_End;
    }
    else
    {
        mcState = mcStart;
    }
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Open(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 开环启动的参数配置
    /*---------------------------------------------------------------------------*/
void Motor_Open(void)
{
    static uint8 OpenRampCycles;
    
    if (McStaSet.SetFlag.StartSetFlag == 0)
    {
        McStaSet.SetFlag.StartSetFlag = 1;
    }
    
    FOC_Init();
    FOC__THETA = _Q15((float)0.0 / 180.0);
    /*********PLL或SMO**********/
    #if ((EstimateAlgorithm == SMO)||(EstimateAlgorithm == AO))
    FOC__ETHETA = FOC__THETA - 4836;    //SMO估算角度延迟
    #elif (EstimateAlgorithm == PLL)
    FOC__ETHETA = FOC__THETA;
    #endif //end    EstimateAlgorithm
    FOC__EOME = 0;
    /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
    FOC_IDREF = ID_Start_CURRENT;                         // D轴启动电流
    mcFocCtrl.mcIqref = IQ_Start_CURRENT;                 // Q轴启动电流
    mcFocCtrl.iiqq = IQ_Start_CURRENT;
    FOC_DKP  = DQKPStart;
    FOC_DKI  = DQKIStart;
    FOC_QKP  = DQKPStart;
    FOC_QKI  = DQKIStart;
    FOC_EKP  = OBSW_KP_GAIN;
    FOC_EKI  = OBSW_KI_GAIN;
    /*启动方式选择*/
    #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);
    #if  (IFFDebugg==1)
    {
        /*估算器禁止输出*/
        ClrBit(FOC_CR1, EFAE);                      // 禁止估算器强制输出
        ClrBit(FOC_CR1, RFAE);                      // 使能强拉
        ClrBit(FOC_CR1, ANGM);                      // 禁止估算器输出
    }
    #endif
    #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_ACC_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_ACC_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状态启动时拖动多次
    
    if (OpenRampCycles < (MOTOR_OPEN_ACC_CYCLE - 1))
    {
        if (!ReadBit(FOC_CR1, RFAE))
        {
            SetBit(FOC_CR1, RFAE);
            OpenRampCycles++;
        }
    }
    else
    {
        mcFocCtrl.State_Count = 2;
        mcState = mcRun;
    }
    
    FOC_EKP = OBSW_KP_GAIN_RUN4;                            // 估算器里的PI的KP
    FOC_EKI = OBSW_KI_GAIN_RUN4;                            // 估算器里的PI的KI
    #elif (Open_Start_Mode == Open_Omega_Start)
    mcFocCtrl.State_Count = 2600;
    mcState = mcRun;
    #elif (Open_Start_Mode == Omega_Start)
    /*********PLL或SMO**********/
    mcFocCtrl.State_Count = 1200;
    mcState   = mcRun;
    #endif //end Open_Start_Mode
    FOC_IQREF = mcFocCtrl.mcIqref;                          // Q轴启动电流
}


/*---------------------------------------------------------------------------*/
/*  Name     :   void MC_Stop(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: inital motor control parameter
    /*---------------------------------------------------------------------------*/
void MC_Stop(void)
{
    MOE     = 0;
    ClrBit(DRV_CR, FOCEN);    //关闭FOC                                                                                                         // disable FOC output and initial register
    mcState = mcInit;
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void VariablesPreInit(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 初始化电机参数
    /*---------------------------------------------------------------------------*/
void VariablesPreInit(void)
{
    /***********保护******************/
    mcFaultSource = 0;
    memset(&mcFaultDect, 0, sizeof(FaultVarible));     // FaultVarible变量清零
    /*****外部控制环*******/
    memset(&mcFocCtrl, 0, sizeof(FOCCTRL));            // mcFocCtrl变量清零
    memset(&mcIimit, 0, sizeof(ILIMIT));            // mcFocCtrl变量清零
    mcFocCtrl.mcDcbus_chazhi = 32760;
    /*****电机状态机时序变量***********/
    McStaSet.SetMode              = 0;   //电流校准标志位置1，其它置0
    /*****LED灯响应***/
    memset(&mcLedDisplay, 0, sizeof(MCLedDisplay));    // mcLedDisplay变量清零
    mcLedDisplay.Counttime        = 4999;
    memset(&VoltageComp, 0, sizeof(VOLCOMP));
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void GetCurrentOffset(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
                调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，OffsetFlag置1。
    /*---------------------------------------------------------------------------*/
void GetCurrentOffset(void)
{
    if (!mcCurOffset.OffsetFlag)
    {
        SetBit(ADC_CR, ADCBSY);             // 使能ADC
        
        while (ReadBit(ADC_CR, ADCBSY));
        
        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_busOffset = mcCurOffset.IvOffset;
        mcCurOffset.OffsetCount++;
        
        if (mcCurOffset.OffsetCount > Calib_Time)
        {
            if (((mcCurOffset.IuOffset > 19959) || (mcCurOffset.IuOffset < 10107)) ||
                ((mcCurOffset.IvOffset > 19959) || (mcCurOffset.IvOffset < 10107)))
            {
                mcFaultSource = FaultIbusOffset;//进入偏置电压错误保护
                FaultProcess();
            }
            else
            {
                mcCurOffset.OffsetFlag = 1;
            }
        }
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Ready(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 上电时，关闭输出，先对硬件电路的电流进行采集，在FOC_Init中写入对应的校准寄存器中。
                调试时，需观察mcCurOffset结构体中对应变量是否在范围内。
    /*---------------------------------------------------------------------------*/
void Motor_Ready(void)
{
    if (McStaSet.SetFlag.CalibFlag == 0)
    {
        McStaSet.SetFlag.CalibFlag = 1;
        ClrBit(DRV_CR, FOCEN);      // 关闭FOC
        MOE = 0;                    // 关闭MOE
        #if (Shunt_Resistor_Mode == Single_Resistor)
        SetBit(ADC_MASK, CH4EN | CH3EN | CH2EN | CH1EN | CH0EN);// 开启ADC
        #else
        SetBit(ADC_MASK, CH4EN | CH1EN | CH0EN); // 开启ADC
        #endif
        mcCurOffset.OffsetCount = 0;
        mcCurOffset.OffsetFlag = 0;                  // 开始电流采集
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Init(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 对电机相关变量、PI进行初始化设置
    /*---------------------------------------------------------------------------*/
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初始化
    #if (FiledWeakenCompEnable==1)
    {
        memset(&mcFieldWeaken, 0, sizeof(FieldWeakeningTypeDef));
    }
    #endif
    #if (IQCOMPENSATEENBLE==1)
    MCTorqueCompensationCodeInit();
    #endif
}