C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE FOCCONTROLFUNCTION
OBJECT MODULE PLACED IN .\Output\FocControlFunction.obj
COMPILER INVOKED BY: F:\keil\C51\BIN\C51.EXE ..\User\Source\Function\FocControlFunction.c LARGE OMF2 WARNINGLEVEL(0) BRO
                    -WSE INCDIR(..\Library\FU68xx_Hardware_Driver\Include;..\User\Include) DEBUG PRINT(.\Listing\FocControlFunction.lst) OBJE
                    -CT(.\Output\FocControlFunction.obj)

line level    source

   1          /**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
   2          * File Name          : FocControl.c
   3          * Author             : Fortiortech  Appliction Team
   4          * Version            : V1.0
   5          * Date               : 10-Apr-2017
   6          * Description        : This file contains all the foc control function used for motor control.
   7          ***************************************************************************************************
   8          * All Rights Reserved
   9          **************************************************************************************************/
  10          
  11          
  12          /* Includes -------------------------------------------------------------------------------------*/
  13          #include <FU68xx_2.h>
  14          #include <Myproject.h>
  15          #include <FU68xx_2_DMA.h>
  16          #endif
  17          CurrentOffset xdata mcCurOffset;
  18          
  19          extern int16 RampStepInc;
  20          
  21          
  22          /*---------------------------------------------------------------------------*/
  23          /* Name         :       void FOC_Init(void)
  24          /* Input        :       NO
  25          /* Output       :       NO
  26          /* Description: mcInit状态下，对FOC的相关寄存器进行配置,先清理寄存器，后配置，�
             -�后使能
  27          /*---------------------------------------------------------------------------*/
  28          void FOC_Init(void)
  29          {
  30   1              /*使能FOC*/
  31   1              ClrBit(DRV_CR, FOCEN);
  32   1              SetBit(DRV_CR, FOCEN);
  33   1              /*配置FOC寄存器*/
  34   1              FOC_CR1                         = 0;                                                                                                                                                    // 清零 FOC_CR1
  35   1              FOC_CR2                         = 0;                                                                                                                                                    // 清零 FOC_CR2
  36   1              FOC_IDREF               = 0;                                                                                                                                                    // 清零 Id
  37   1              FOC_IQREF               = 0;                                                                                                                                                    // 清零 Iq
  38   1      
  39   1              FOC__THETA              = 0;                                                                                                                                                    // 清零 角度
  40   1              FOC_RTHEACC     = 0;                                                                                                                                                    // 清零 爬坡函数的初始加速度
  41   1              FOC__RTHESTEP = 0;                                                                                                                                              
  42   1              FOC__RTHECNT    = 0;                                                                                                                                                    
  43   1              FOC__THECOMP    = _Q15(0.0/180.0);                                                                                              // SMO 估算补偿角
  44   1              FOC__THECOR             = 0x04;                                                                                                                                 // 误差角度补偿
  45   1              
  46   1              /*电流环参数配置*/
  47   1              
  48   1              
  49   1              #if (CurrentCycleEnable == 1)//使能电流环
                      {
                                      ClrBit(FOC_CR2, UQD);
                                      ClrBit(FOC_CR2, UDD);
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 2   

                      }
                      #elif (CurrentCycleEnable == 0)//不使能电流环
  55   1              {
  56   2                              SetBit(FOC_CR2, UQD);
  57   2                              SetBit(FOC_CR2, UDD);
  58   2              }
  59   1              #endif //end SVPMW_Mode
  60   1              
  61   1              
  62   1              FOC_DMAX                        = DOUTMAX;
  63   1              FOC_DMIN                        = DOUTMIN;
  64   1      
  65   1              FOC_QMAX                        = QOUTMAX;
  66   1              FOC_QMIN                        = QOUTMIN;
  67   1      
  68   1                      //FOC__UQ = StartPWMDuty;
  69   1      
  70   1      //      FOC_QMAX                        = StartPWMDuty;
  71   1      //      FOC_QMIN                        = StartPWMDuty;
  72   1      
  73   1              /*位置估算参数配置*/
  74   1              FOC_EK1                         = OBS_K1T;
  75   1              FOC_EK2                         = OBS_K2T;
  76   1              FOC_EK3                         = OBS_K3T;
  77   1              FOC_EK4                         = OBS_K4T;
  78   1              FOC_FBASE               = OBS_FBASE;
  79   1              FOC_OMEKLPF     = SPEED_KLPF;
  80   1              FOC_EBMFK               = OBS_KLPF;
  81   1      
  82   1        FOC_TGLI      = PWM_TGLI_LOAD;
  83   1      
  84   1              /*********PLL或SMO**********/
  85   1              #if (EstimateAlgorithm == SMO)
  86   1              {
  87   2                              ClrBit(FOC_CR2, ESEL);
  88   2                              FOC_KSLIDE    = OBS_KSLIDE;
  89   2                              FOC_EKLPFMIN    = OBS_EA_KS;
  90   2              }
  91   1              #elif (EstimateAlgorithm == PLL)
                      {
                                      SetBit(FOC_CR2, ESEL);
                                      FOC_KSLIDE    = OBSE_PLLKP_GAIN;
                                      FOC_EKLPFMIN    = OBSE_PLLKI_GAIN;
                      }
                      #endif //end SVPMW_Mode
  98   1      
  99   1              SetBit(FOC_CR1, SVPWMEN);                                                                                                                       // SVPWM模式
 100   1      
 101   1      
 102   1              /**过调制**/
 103   1              #if (OverModulation == 1)
                      {
                                      SetBit(FOC_CR1,OVMDL);                                                                                                  // 过调制
                      }
                      #endif //end OverModulation
 108   1      
 109   1              /*单电阻采样；需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式*/
 110   1              #if (Shunt_Resistor_Mode == Single_Resistor)
 111   1              {
 112   2                      SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
 113   2                      FOC_TSMIN = PWM_TS_LOAD;                                                                                                                        // 最小采样窗口
 114   2                      FOC_TRGDLY = 0x05;                                                                                                              // 采样时刻在中点，一般考虑开关噪声影响，会
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 3   

             -设置延迟；
 115   2                                                                            // 0x0c表示延迟12个clock，提前用反码形
             -式，如0x84表示提前12个clock。
 116   2                      ClrBit(FOC_CR2,F5SEG);                                                                                                                    // 7段式
 117   2                      SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
 118   2              }
 119   1              /*双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia，配置81*/
 120   1              #elif (Shunt_Resistor_Mode == Double_Resistor)                                  // double resistor sample
                      {
                              SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
              
                              FOC_TSMIN = PWM_DT_LOAD;                                                                                                                        // 死区补偿值
                              FOC_TRGDLY = 0x80;                                                                                                                                        // ADC采样的时刻，采样时刻在计数器零点附近，83�
             -�下降沿结束前3个clock采样Ia，与单电阻不同
                                                                                    // 01为上升沿开始后第一个clock开始采�
             -��。根据实际情况调整。
                              FOC_TBLO=PWM_DLOWL_TIME;                              //下桥臂最小脉冲，保证采样
                              SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
              
                              /*五段式或七段式选择*/
                              #if (SVPMW_Mode == SVPWM_7_Segment)
                              {
                                      ClrBit(FOC_CR2,F5SEG);                                                                                                                  // 7段式
                              }
                              #elif (SVPMW_Mode == SVPWM_5_Segment)
                              {
                                      SetBit(FOC_CR2,F5SEG);                                                                                                                  // 7段式
                              }
                              #endif
              
                              #if (DouRes_Sample_Mode == DouRes_1_Cycle)
                              {
                                      ClrBit(FOC_CR2,DSS);                                                                                                                      // 7段式
                              }
                              #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
                              {
                                      SetBit(FOC_CR2,FOC_DSS);                                                                                                                // 7段式
                              }
                              #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 =06;                                                                                                                                                   // ADC采样的时刻，采样时刻在计数器零点附近，83为
             -下降沿结束前3个clock采样Ia，与单电阻不同。
                                                                                    // 01为上升沿开始后第一个clock开始采�
             -��。根据实际情况调整。
              
                              SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, CMP3MOD0 | CMP3MOD1);
                              FOC_TBLO= PWM_OVERMODULE_TIME;                        // 过调制电流采样处理的TB脉宽
              
                              /*五段式或七段式选择*/
                              #if (SVPMW_Mode == SVPWM_7_Segment)
                              {
                                      ClrBit(FOC_CR2,F5SEG);                                                                                                                  // 7段式
                              }
                              #elif (SVPMW_Mode == SVPWM_5_Segment)
                              {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 4   

                                      SetBit(FOC_CR2,F5SEG);                                                                                                                  // 7段式
                              }
                              #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);                                                                                                                    // 7段式
                              }
                              #endif //end DouRes_Sample_Mode
                      }
                      #endif  //end Shunt_Resistor_Mode
 186   1      
 187   1              /* 使能电流基准校正     */
 188   1              #if (CalibENDIS == Enable)
 189   1              {
 190   2                      if(mcCurOffset.OffsetFlag>=1)
 191   2                      {
 192   3                              #if (Shunt_Resistor_Mode == Single_Resistor)          // 单电阻校正
 193   3                              {
 194   4                                      /*set ibus current sample offset*/
 195   4                                      SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
 196   4                                      FOC_CSO = mcCurOffset.Iw_busOffset;                   // 写入Ibus的偏置
 197   4      
 198   4                              }
 199   3                              #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
 222   3                      }
 223   2              }
 224   1              #endif  //end CalibENDIS
 225   1              /*-------------------------------------------------------------------------------------------------
 226   1              DRV_CTL：PWM来源选择
 227   1              OCS = 0, DRV_COMR
 228   1              OCS = 1, FOC/SVPWM/SPWM
 229   1              -------------------------------------------------------------------------------------------------*/
 230   1              /*计数器比较值来源FOC*/
 231   1              SetBit(DRV_CR, OCS);
 232   1      }
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 5   

 233          /*---------------------------------------------------------------------------*/
 234          /* Name         :       void Motor_Charge(void)
 235          /* Input        :       NO
 236          /* Output       :       NO
 237          /* Description: 预充电，当一直处于预充电状态下，不接电机，可用于验证IPM或者Mos
             -。
 238          预充电分三步，第一步是对U相进行预充电，第二步是对U,V两相进行预充电;第三�
             -�是对U、V、W三相进行预充电。
 239          /*---------------------------------------------------------------------------*/
 240          void Motor_Charge(void)
 241          {
 242   1              if(McStaSet.SetFlag.ChargeSetFlag==0)
 243   1              {
 244   2                      McStaSet.SetFlag.ChargeSetFlag = 1;
 245   2                      #if (PWM_Level_Mode == Low_Level)        //用于測試6536
                              {
                                      SetBit(P1_OE, P10);
                                      GP10=1;
                              }
                              #endif
 251   2                      #if (IPMState == IPMtest)
                              {
                                      DRV_DR = 0.7*DRV_ARR;                                               // IPM 70% duty
                              }
                              #elif (IPMState == NormalRun)                 // 正常按电机状态机运行
 256   2                      {
 257   3                              DRV_DR = 0.1*DRV_ARR;                                   //下桥臂10% duty
 258   3                      }
 259   2                      #endif
 260   2                      /*-------------------------------------------------------------------------------------------------
 261   2                      DRV_CTL：PWM来源选择
 262   2                      OCS = 0, DRV_COMR
 263   2                      OCS = 1, FOC/SVPWM/SPWM
 264   2                      -------------------------------------------------------------------------------------------------*/
 265   2                      ClrBit(DRV_CR, OCS);
 266   2                      mcFocCtrl.ChargeStep = 0;
 267   2              }
 268   1              if((mcFocCtrl.State_Count < Charge_Time)&&(mcFocCtrl.ChargeStep == 0))
 269   1              {
 270   2                      mcFocCtrl.ChargeStep = 1;
 271   2                      #if (IPMState == IPMtest)
                              {
                                      DRV_CMR |= 0x03;                         // U相输出
                              }
                              #elif (IPMState == NormalRun)                     // 正常按电机状态机运行
 276   2                      {
 277   3                              DRV_CMR |= 0x01;                         // U相下桥臂通
 278   3                      }
 279   2                      #endif
 280   2                      MOE = 1;
 281   2              }
 282   1              if(( mcFocCtrl.State_Count <= (Charge_Time<<1)/3)&&(mcFocCtrl.ChargeStep== 1))
 283   1              {
 284   2                      mcFocCtrl.ChargeStep = 2;
 285   2                      #if (IPMState == IPMtest)
                              {
                                      DRV_CMR |= 0x0F;                         // U、V相输出
                              }
                              #elif (IPMState == NormalRun)                     // 正常按电机状态机运行
 290   2                      {
 291   3                              DRV_CMR |= 0x04;                         // V相下桥臂导通
 292   3                      }
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 6   

 293   2                      #endif
 294   2              }
 295   1              if((mcFocCtrl.State_Count <= Charge_Time/3)&&(mcFocCtrl.ChargeStep== 2))
 296   1              {
 297   2                      mcFocCtrl.ChargeStep = 3;
 298   2                      #if (IPMState == IPMtest)
                              {
                              DRV_CMR |= 0x3F;                         // U、V、W相输出
                              }
                              #elif (IPMState == NormalRun)                     // 正常按电机状态机运行
 303   2                      {
 304   3                      DRV_CMR |= 0x10;                         // W相下桥臂导通
 305   3                      }
 306   2                      #endif
 307   2              }
 308   1      }
 309          /*---------------------------------------------------------------------------*/
 310          /* Name         :       void Motor_Align(void)
 311          /* Input        :       NO
 312          /* Output       :       NO
 313          /* Description: 预定位函数，当无逆风判断时，采用预定位固定初始位置;当有逆风�
             -�断时，采用预定位刹车
 314          /*---------------------------------------------------------------------------*/
 315          void Motor_Align(void)
 316          {
 317   1              if(McStaSet.SetFlag.AlignSetFlag==0)
 318   1              {
 319   2                        McStaSet.SetFlag.AlignSetFlag=1;
 320   2      
 321   2                        /*FOC初始化*/
 322   2                              FOC_Init();
 323   2      
 324   2                        /*配置预定位的电流、KP、KI*/
 325   2                              FOC_IDREF = ID_Align_CURRENT;
 326   2                              FOC_IQREF = IQ_Align_CURRENT;
 327   2      
 328   2                              FOC_DQKP = DQKP_Alignment;
 329   2                              FOC_DQKI = DQKI_Alignment;
 330   2      
 331   2                              FOC_EKP         = OBSW_KP_GAIN;
 332   2                              FOC_EKI         = OBSW_KI_GAIN;
 333   2      
 334   2            /*配置预定位角度*/
 335   2                              #if (AlignTestMode==1)
                                      {
                                              FOC__THETA              = Align_Theta;
                                      }
                                      #else
 340   2                              {
 341   3                                      #if (PosCheckEnable==1)
                                              {
                                                FOC__THETA    = mcFocCtrl.mcPosCheckAngle;
                                              }
                                              #else
 346   3                                      {
 347   4                                              FOC__THETA              =Align_Theta;
 348   4                                      }
 349   3                                #endif        //end PosCheckEnable
 350   3                              }
 351   2                              #endif  //end AlignTestMode
 352   2                                                      /*********PLL或SMO**********/
 353   2                              #if (EstimateAlgorithm == SMO)
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 7   

 354   2                              {
 355   3                                FOC__ETHETA   = FOC__THETA-4096;
 356   3                              }
 357   2                              #elif (EstimateAlgorithm == PLL)
                                      {
                                        FOC__ETHETA   = FOC__THETA;
                                      }
                                      #endif //end    EstimateAlgorithm
 362   2      //                      TimeCnt=FOC__THETA;
 363   2      
 364   2                        /*使能输出*/
 365   2                              DRV_CMR |= 0x3F;                         // U、V、W相输出
 366   2                              MOE = 1;
 367   2        }
 368   1      }
 369          
 370          /*---------------------------------------------------------------------------*/
 371          /* Name         :       void Motor_Open(void)
 372          /* Input        :       NO
 373          /* Output       :       NO
 374          /* Description: 开环启动的参数配置
 375          /*---------------------------------------------------------------------------*/
 376          void Motor_Open(void)
 377          {
 378   1              static uint8 OpenRampCycles;
 379   1              
 380   1              uint8 hall_status= 0;
 381   1              
 382   1              if(MotorStateTime.OpenOneTime==0)
 383   1              {
 384   2                      MotorStateTime.OpenOneTime=1;
 385   2                      
 386   2                      /*FOC初始化*/
 387   2                      FOC_Init();
 388   2                                                              
 389   2                      /*********PLL或SMO**********/
 390   2                      #if (EstimateAlgorithm == SMO)
 391   2                      {
 392   3                              FOC__ETHETA     = FOC__THETA - 4915;
 393   3                      }
 394   2                      #elif (EstimateAlgorithm == PLL)
                              {
                                      FOC__ETHETA     = FOC__THETA;
                              }
                              #endif //end    EstimateAlgorithm
 399   2      
 400   2      //              TimeCnt=FOC__ETHETA;                                                    
 401   2                      /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
 402   2                      FOC_IDREF = ID_Start_CURRENT;               // D轴启动电流
 403   2                      mcFocCtrl.mcIqref= IQ_Start_CURRENT;                // Q轴启动电流
 404   2                      
 405   2                      FOC_IQREF = mcFocCtrl.mcIqref;                    // Q轴启动电流
 406   2      
 407   2                      FOC_DQKP        = DQKP;
 408   2                      FOC_DQKI        = DQKI;
 409   2      
 410   2                      FOC_EKP         = OBSW_KP_GAIN;
 411   2                      FOC_EKI         = OBSW_KI_GAIN;
 412   2      
 413   2      
 414   2                      /*启动方式选择*/
 415   2                      #if (Open_Start_Mode == Omega_Start)            // Omega 启动
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 8   

                              {
                                      FOC_EFREQACC    = Motor_Omega_Ramp_ACC;
                                      FOC_EFREQMIN    = Motor_Omega_Ramp_Min;
                                      FOC_EFREQHOLD   = Motor_Omega_Ramp_End;
              
                                      SetBit(FOC_CR1,EFAE);                                           // 估算器强制输出
                                      ClrBit(FOC_CR1,RFAE);                                           // 禁止强拉
                                      SetBit(FOC_CR1,ANGM);                                           // 估算模式
                              }
                              #elif (Open_Start_Mode == Open_Start)
 426   2                      {
 427   3      //                      FOC_RTHEACC     = Motor_Open_Ramp_ACC;          // 爬坡函数的初始加速度
 428   3      //                      FOC__RTHESTEP   = Motor_Open_Ramp_Min;          // 0.62 degree acce speed
 429   3      //                      RPDPara.ThetaGet=Motor_Open_Ramp_Min;
 430   3      //                      FOC__RTHECNT    = MOTOR_OPEN_ACC_CNT;           // acce time
 431   3      
 432   3                              ClrBit(FOC_CR1,EFAE);                                           // 估算器强制输出
 433   3                              ClrBit(FOC_CR1,RFAE);                                           // 禁止强拉
 434   3                              ClrBit(FOC_CR1,ANGM);                                           // 估算模式
 435   3                      }
 436   2                      #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;
              
                                      SetBit(FOC_CR1,EFAE);                                           // 估算器强制输出
                                      SetBit(FOC_CR1,RFAE);                                           // 禁止强拉
                                      SetBit(FOC_CR1,ANGM);                                           // 估算模式
                              }
                              #endif //end Open_Start_Mode
 451   2              }
 452   1              
 453   1              /*不同启动方式下，切换到MCRUN状态*/
 454   1      //      #if (Open_Start_Mode == Open_Start)                                     //OPEN状态启动时拖动多次
 455   1      //      {
 456   1      //              if(OpenRampCycles<(MOTOR_OPEN_ACC_CYCLE-1))
 457   1      //              {
 458   1      //                      if(!ReadBit(FOC_CR1,RFAE))
 459   1      //                      {
 460   1      //                              SetBit(FOC_CR1,RFAE);
 461   1      //                              OpenRampCycles++;
 462   1      //                      }
 463   1      //              }
 464   1      //              else
 465   1      //              {
 466   1      //                      mcFocCtrl.State_Count = 2;                                      //???这里为什么延时2ms
 467   1      
 468   1      //                      mcState = mcRun;
 469   1      //              }
 470   1      //              FOC_EKP = OBSW_KP_GAIN_RUN4;                    // 估算器里的PI的KP
 471   1      //              FOC_EKI = OBSW_KI_GAIN_RUN4;                                // 估算器里的PI的KI
 472   1      //      }
 473   1      //      #elif (Open_Start_Mode == Open_Omega_Start)
 474   1      //      {
 475   1      //              mcFocCtrl.State_Count = 2600;
 476   1      //              mcState = mcRun;
 477   1      //      }
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 9   

 478   1      //      #elif (Open_Start_Mode == Omega_Start)
 479   1      //      {
 480   1      //              /*********PLL或SMO**********/
 481   1      //              #if (EstimateAlgorithm == SMO)
 482   1      //              {
 483   1      //                      mcFocCtrl.State_Count = 2600;
 484   1      //              }
 485   1      //              #elif (EstimateAlgorithm == PLL)
 486   1      //              {
 487   1      //                      mcFocCtrl.State_Count = 1400;
 488   1      //                      FOC_EKP = OBSW_KP_GAIN_RUN1;                    // 估算器里的PI的KP
 489   1      //                      FOC_EKI = OBSW_KI_GAIN_RUN1;                            // 估算器里的PI的KI
 490   1      //              }
 491   1      //              #endif //end    EstimateAlgorithm
 492   1      //              
 493   1      //      }
 494   1      //      #endif //end Open_Start_Mode
 495   1              
 496   1              if(HALLC)
 497   1              {
 498   2                      hall_status += 4;
 499   2              }
 500   1              
 501   1              if(HALLB)
 502   1              {
 503   2                      hall_status += 2;
 504   2              }
 505   1              
 506   1              if(HALLA)
 507   1              {
 508   2                      hall_status += 1;
 509   2              }
 510   1                      
 511   1              mcHall.HallStatus_Result = hall_status;
 512   1              
 513   1              FOC__RTHESTEP                  = 0; 
 514   1              
 515   1              FOC__THETA = StartThetaDispose (mcHall.HallStatus_Result,mcHall.GivenFRStatus,TempAngleOffsetCW,TempAngl
             -eOffsetCCW,mcHall.StartAngleOffset);
 516   1                                      
 517   1              mcState = mcRun;
 518   1              
 519   1              mcHall.HallCommutationCount   = 1;
 520   1              
 521   1              DRV_CMR |= 0x3F;                         // U、V、W相输出
 522   1                      
 523   1      //      if(mcHall.HallDirectStartStatus)
 524   1      //      {
 525   1      //              MOE = 0;                
 526   1      //      }
 527   1      //      else
 528   1      //      {
 529   1                      MOE = 1;
 530   1                                              
 531   1                      //FOC__UQ = 0;
 532   1      //              FOC_QMAX                        = 0;
 533   1      //              FOC_QMIN                        = 0;
 534   1      //      }
 535   1      }
 536          
 537          /*---------------------------------------------------------------------------*/
 538          /* Name         :       void Motor_Open(void)
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 10  

 539          /* Input        :       NO
 540          /* Output       :       NO
 541          /* Description: 开环启动的参数配置
 542          /*---------------------------------------------------------------------------*/
 543          void Motor_PllStart(void)
 544          {
 545   1              static uint8 OpenRampCycles;
 546   1              if(TailWindDetect.PLLFlag==1)
 547   1              {
 548   2                      TailWindDetect.PLLSpeed=FOC__EOME;
 549   2              TailWindDetect.PLLSpeed=Abs_F16(TailWindDetect.PLLSpeed);
 550   2                      if(Abs_F16(TailWindDetect.PLLSpeed)<350)                //逆风状态下，速度低于100时，计数，达到一定
             -时间后，PLLFlag清零。
 551   2                      {
 552   3                              TailWindDetect.PLLDetectCnt++;
 553   3                              if(TailWindDetect.PLLDetectCnt>3000)            //防止估算错误
 554   3                              {
 555   4                                      TailWindDetect.PLLFlag=0;
 556   4                                      TailWindDetect.PLLTheta=FOC__ETHETA;
 557   4      //                              FOC_IQREF =0;
 558   4                              }
 559   3                      }
 560   2                      else
 561   2                      {
 562   3                              if(TailWindDetect.PLLDetectCnt>0)
 563   3                              {
 564   4                                      TailWindDetect.PLLDetectCnt--;
 565   4                              }
 566   3                      }
 567   2              }
 568   1              if((McStaSet.SetFlag.StartSetFlag==0)&&(TailWindDetect.PLLFlag==0))
 569   1              {
 570   2                      McStaSet.SetFlag.StartSetFlag=1;
 571   2                      MOE = 0;
 572   2                      FOC_Init();
 573   2                      FOC__ETHETA             = TailWindDetect.PLLTheta;//+19668;//16668;//
 574   2      
 575   2                      /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
 576   2                      FOC_IDREF = ID_Start_CURRENT;                   // D轴启动电流
 577   2                      FOC_IQREF = mcFocCtrl.mcIqref;                      // Q轴启动电流
 578   2      
 579   2                      FOC_DQKP = _Q12(2.0) ;//DQKPStart;
 580   2                      FOC_DQKI = _Q15(0.16) ;//DQKIStart;
 581   2      //        FOC__UQ   =_Q15(0.3);
 582   2      
 583   2                      /*启动方式选择*/
 584   2                      #if (Open_Start_Mode == Omega_Start)            // Omega 启动
                              {
                                      FOC_EFREQACC    = Motor_Omega_Ramp_ACC_Antiwind;
                                      FOC_EFREQMIN    = 3*Motor_Omega_Ramp_Min;
                                      FOC_EFREQHOLD   = Motor_Omega_Ramp_End;
              
                                      SetBit(FOC_CR1,EFAE);                                           // 禁止估算器强制输出
                                      ClrBit(FOC_CR1,RFAE);                                           // 使能强拉
                                      SetBit(FOC_CR1,ANGM);                                           // 估算模式
                              }
                              #elif (Open_Start_Mode == Open_Start)
 595   2                      {
 596   3      //                      FOC_RTHEACC     = Motor_Open_Ramp_ACC;          // 爬坡函数的初始加速度
 597   3      //                      FOC__RTHESTEP   = Motor_Open_Ramp_Min;          // 0.62 degree acce speed
 598   3      //                      FOC__RTHECNT    = MOTOR_OPEN_ACC_CNT;           // acce time
 599   3      
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 11  

 600   3                              ClrBit(FOC_CR1,EFAE);                                           // 禁止估算器强制输出
 601   3                              ClrBit(FOC_CR1,RFAE);                                           // 使能强拉
 602   3                              ClrBit(FOC_CR1,ANGM);                                           // 估算模式
 603   3                      }
 604   2                      #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;
              
                                      SetBit(FOC_CR1,EFAE);                                           // 禁止估算器强制输出
                                      SetBit(FOC_CR1,RFAE);                                           // 使能强拉
                                      SetBit(FOC_CR1,ANGM);                                           // 估算模式
                              }
                              #endif //end Open_Start_Mode
 619   2                      DRV_CMR |= 0x3F;                                        // U、V、W相输出
 620   2                      MOE = 1;
 621   2              }
 622   1      
 623   1              if((McStaSet.SetFlag.StartSetFlag==1)&&(TailWindDetect.PLLFlag==0))
 624   1              {
 625   2                      /*不同启动方式下，切换到MCRUN状态*/
 626   2                      #if (Open_Start_Mode == Open_Start)                             //OPEN状态启动时拖动多次
 627   2                      {
 628   3                              if(OpenRampCycles<(MOTOR_OPEN_ACC_CYCLE-1))
 629   3                              {
 630   4                                      if(!ReadBit(FOC_CR1,RFAE))
 631   4                                      {
 632   5                                              SetBit(FOC_CR1,RFAE);
 633   5                                              OpenRampCycles++;
 634   5                                      }
 635   4                              }
 636   3                              else
 637   3                              {
 638   4                                      mcFocCtrl.State_Count = 2;
 639   4                                      mcState = mcRun;
 640   4                              }
 641   3                              FOC_EKP = OBSW_KP_GAIN_RUN4;                // 估算器里的PI的KP
 642   3                              FOC_EKI = OBSW_KI_GAIN_RUN4;                            // 估算器里的PI的KI
 643   3                      }
 644   2                      #elif (Open_Start_Mode == Open_Omega_Start)
                              {
                                      mcFocCtrl.State_Count = 2600;
                                      mcState = mcRun;
                              }
                              #elif (Open_Start_Mode == Omega_Start)
                              {
                                      mcFocCtrl.State_Count = 2900;
                                      FOC_EKP = OBSW_KP_GAIN;             // 估算器里的PI的KP
                                      FOC_EKI = OBSW_KI_GAIN;                         // 估算器里的PI的KI
                                      mcState = mcRun;
                              }
                              #endif //end Open_Start_Mode
 657   2              }
 658   1      
 659   1      }
 660          /*---------------------------------------------------------------------------*/
 661          /* Name         :       void Motor_Align(void)
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 12  

 662          /* Input        :       NO
 663          /* Output       :       NO
 664          /* Description: 顺风逆风参数配置函数
 665          /*---------------------------------------------------------------------------*/
 666          void Motor_TailWind(void)
 667          {
 668   1              if(mcFocCtrl.State_Count == 0)
 669   1              {
 670   2                      if(McStaSet.SetFlag.TailWindSetFlag  ==0)//初始化
 671   2                      {
 672   3                              McStaSet.SetFlag.TailWindSetFlag =1;
 673   3      
 674   3                              #if (FRDetectMethod==RSDMethod)
                                      {
                                              RSDDetectInit();
                                      }
                                      #elif (FRDetectMethod==BEMFMethod)
                                      {
                                              BEMFDetectInit();
                                      }
                                      #elif (FRDetectMethod==FOCMethod)
 683   3                              {
 684   4                                      TailWindDetectInit();
 685   4                              }
 686   3                              #endif
 687   3      
 688   3                      }
 689   2                      
 690   2                      #if (FRDetectMethod==RSDMethod)
                              {
                                      RSDDealwith();
                              }
                              #elif (FRDetectMethod==BEMFMethod)
                              {
                                      BEMFDealwith();
                              }
                              #elif (FRDetectMethod==FOCMethod)
 699   2                      {
 700   3                              FOCTailWindDealwith();
 701   3                      }
 702   2                      #endif
 703   2              }
 704   1      }
 705          
 706          /*---------------------------------------------------------------------------*/
 707          /* Name         :       void MC_Stop(void)
 708          /* Input        :       NO
 709          /* Output       :       NO
 710          /* Description: inital motor control parameter
 711          /*---------------------------------------------------------------------------*/
 712          void MC_Stop(void)
 713          {
 714   1              MOE     = 0;
 715   1              ClrBit(DRV_CR, FOCEN, 0);       //关闭FOC                                                                                                                                                                                                                     // disable FOC output and initial registe
             -r
 716   1              mcState = mcInit;
 717   1      }
 718          
 719          /*-------------------------------------------------------------------------------------------------
 720                  Function Name : void MotorControlInit(void)
 721                  Description   : 控制变量初始化清零,包括保护参数的初始化、电机状态初始化
 722                  Input         : 输入说明（详细）
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 13  

 723            Output                                :       输出说明（详细）
 724          -------------------------------------------------------------------------------------------------*/
 725          void MotorcontrolInit(void)
 726          {
 727   1        /***********保护******************/
 728   1              memset(&mcFaultDect,0, sizeof(FaultVarible));                                                                                                                           // FaultVarible变量清零
 729   1        /******堵转保护*********/
 730   1      //      mcFaultDect.StallRecover          = 1000;//5s
 731   1      
 732   1        /******保护次数*********/
 733   1              memset(&mcProtectTime,0, sizeof(ProtectVarible));                                                                                                               // ProtectVarible保护次数清零 
 734   1        /*******过流保护*****************/
 735   1              memset(&mcCurVarible,0, sizeof(CurrentVarible));                                                                                                                // 电流保护的变量清零
 736   1      
 737   1        /*****顺逆风判断，FOCMethod不需要在此赋初值*******/
 738   1              #if (FRDetectMethod==RSDMethod)
                      {
                 memset(&RSDDetect,0, sizeof(MotorRSDTypeDef));       //      RSDDetect所有变量清零
                      }
                      #elif (FRDetectMethod==BEMFMethod)
                      {
                 memset(&BEMFDetect,0, sizeof(BEMFDetect_TypeDef));           //      BEMFDetect所有变量清零
                      } 
                #endif
 747   1      
 748   1              /*******启停测试的参数***************/
 749   1              memset(&ONOFFTest,0, sizeof(ONVarible));
 750   1      
 751   1        /*****电机状态机时序变量***********/
 752   1              McStaSet.SetMode                   = 0;
 753   1      
 754   1        /*****电机目标方向**********/
 755   1              mcFRState.TargetFR                 = 0;
 756   1      
 757   1        /*****外部控制环*******/
 758   1              memset(&mcFocCtrl,0, sizeof(FOCCTRL));                                                                                                                          // mcFocCtrl变量清零
 759   1      
 760   1        /******ADC采样滤波值*********/
 761   1              memset(&AdcSampleValue,0, sizeof(ADCSample));                                                                                                   // ADCSample变量清零
 762   1      
 763   1        /******电流偏置校准变量*****/
 764   1              memset(&mcCurOffset,0, sizeof(CurrentOffset));                                                                                          // mcCurOffset变量清零
 765   1              mcCurOffset.IuOffsetSum            = 16383;
 766   1              mcCurOffset.IvOffsetSum            = 16383;
 767   1              mcCurOffset.Iw_busOffsetSum        = 16383;
 768   1      
 769   1              /*****LED灯响应***/
 770   1              memset(&mcLedDisplay,0, sizeof(MCLedDisplay));                                                                                          // mcLedDisplay变量清零
 771   1              mcLedDisplay.Counttime            = 4999;
 772   1      
 773   1              /*****速度环的响应***/
 774   1              memset(&mcSpeedRamp,0, sizeof(MCRAMP));                                                                                                 // mcSpeedRamp变量清零
 775   1      //      mcSpeedRamp.IncValue              = Motor_Speed_Inc;
 776   1      //      mcSpeedRamp.DecValue              = Motor_Speed_Dec;
 777   1      //      mcSpeedRamp.DelayPeriod           = 1;
 778   1      
 779   1              /*PWM调速变量*/
 780   1              memset(&mcPwmInput,0, sizeof(PWMINPUTCAL));                                                                                                 // mcPwmInput变量清零
 781   1      
 782   1              /*串口变量*/
 783   1              memset(&Uart,0, sizeof(MCUART));                                                                                                              // MCUART变量清零
 784   1      
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 14  

 785   1              /*睡眠模式*/  
 786   1              memset(&SleepSet,0, sizeof(SLEEPMODE));                                     // 睡眠模式清零
 787   1              
 788   1              memset(&MotorStateTime,0, sizeof(MotStaTim));                           //清零
 789   1              
 790   1              memset(&mcHall,0, sizeof(mcHall_TypeDef));                                //清零
 791   1              
 792   1              /*****环路响应***/  
 793   1              #if (Speed_Loop_Mode == Speed_OpenLoop_Enable)
 794   1              SpeedRamp.RampInc                                                               = Motor_Voltage_Inc;
 795   1              SpeedRamp.RampDec                                                                       =       Motor_Voltage_Dec;
 796   1              SpeedRamp.RampTarget                                                    =       StartPWMDuty;
 797   1              SpeedRamp.RampStep                                                              =       MinPWMDuty;
 798   1              SpeedRamp.SPEEDRampTIMETemp       =  SPEEDRampTIME;
 799   1              #endif
 800   1              
 801   1      }
 802          
 803          /*---------------------------------------------------------------------------*/
 804          /* Name         :       void VariablesPreInit(void)
 805          /* Input        :       NO
 806          /* Output       :       NO
 807          /* Description: 初始化电机参数
 808          /*---------------------------------------------------------------------------*/
 809          void VariablesPreInit(void)
 810          {
 811   1              /*************UQ爬坡速度**************/
 812   1              RampStepInc = 120;
 813   1              
 814   1              /***********保护******************/
 815   1              mcFaultSource = 0;
 816   1              
 817   1              memset(&mcFaultDect,0, sizeof(FaultVarible));                                                                                           // FaultVarible变量清零
 818   1      
 819   1        /*****外部控制环*******/
 820   1      //      memset(&mcFocCtrl,0, sizeof(FOCCTRL));                                                                                                          // mcFocCtrl变量清零
 821   1              mcFocCtrl.FreeStopDecStatus        = 0;
 822   1        
 823   1              mcFocCtrl.OpenDutyCycle            = 0;
 824   1      
 825   1              mcFocCtrl.RMSLimitTargetCurrent    = IQ_RUN_MAX_RMS_CURRENT; 
 826   1              
 827   1      //      mcFocCtrl.mcBrakeStatus          = 0;
 828   1                      
 829   1              mcFocCtrl.mcSpeedVSP               = 0;
 830   1              
 831   1              mcFocCtrl.mcADCVoltagebus          = 0;
 832   1              
 833   1              mcFocCtrl.OverCurrentFlag          = 0;
 834   1              
 835   1              mcFocCtrl.mcBlackOffStopFlag =0;   
 836   1              
 837   1              mcFocCtrl.mcADCVoltagebus                                        = 0;
 838   1      
 839   1              mcFocCtrl.mcADCCurrentbus                                        = 0;
 840   1              
 841   1              mcFocCtrl.mcADCProtectCurrentbus   = 0;  
 842   1      
 843   1        /*****顺逆风判断*******/
 844   1              #if (FRDetectMethod==RSDMethod)
                      {
                        RSDDetect.RSDCCWTimes    = 0;                             //  CCW刹车次数清零
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 15  

                      }
                      #elif (FRDetectMethod==BEMFMethod)
                      {
                              
                      }
                      #elif (FRDetectMethod==FOCMethod)
 853   1              {
 854   2               memset(&TailWindDetect,0, sizeof(MotorTailWindTypeDef));               //      TailWindDetect所有变量清零
 855   2              }
 856   1        #endif
 857   1        /*****电机状态机时序变量***********/
 858   1        McStaSet.SetMode                   = 0;
 859   1      
 860   1              /*****LED灯响应***/
 861   1              memset(&mcLedDisplay,0, sizeof(MCLedDisplay));                                                                                          // mcLedDisplay变量清零
 862   1              mcLedDisplay.Counttime            = 4999;
 863   1              
 864   1              memset(&MotorStateTime,0, sizeof(MotStaTim));                           //清零
 865   1              
 866   1              mcHall.HALLLockHCTime             = 0;
 867   1        mcHall.HALLLockHBTime             = 0;
 868   1              mcHall.HALLLockHATime             = 0;
 869   1              mcHall.HALLHACWCCWTime            = 0;
 870   1              mcHall.HALLHBCWCCWTime            = 0;
 871   1              mcHall.HALLHCCWCCWTime            = 0;
 872   1              mcHall.HALLLockStatus             = 0;
 873   1              mcHall.mcStallTimeCnt             = 0;
 874   1              mcHall.mcFallWarningSignal        = 1;   
 875   1              mcHall.RTheStep                   = 0;  
 876   1              mcHall.PeriodTime                 = 0;  
 877   1              mcHall.AngleCompensate            = 0;
 878   1              mcHall.HallCommutationCnt         = 0;  
 879   1      //      mcHall.HallFRreverse               =0;  
 880   1              mcHall.HallDirectStartStatus                     = 0;                           
 881   1              mcHall.StartAngleOffset                                    = 0;
 882   1              mcHall.HallSpeedInitStatus         = 0;
 883   1              
 884   1              /********环路响应*********/ 
 885   1              #if (Speed_Loop_Mode == Speed_OpenLoop_Enable)
 886   1              SpeedRamp.RampInc                                                               = Motor_Voltage_Inc;
 887   1              SpeedRamp.RampDec                                                                       =       Motor_Voltage_Dec;
 888   1              SpeedRamp.RampTarget                                                    =       StartPWMDuty;
 889   1              SpeedRamp.RampStep                                                              =       MinPWMDuty;
 890   1              SpeedRamp.SPEEDRampTIMETemp       =  SPEEDRampTIME;
 891   1              #endif
 892   1              
 893   1      }
 894          /*---------------------------------------------------------------------------*/
 895          /* Name         :       void GetCurrentOffset(void)
 896          /* Input        :       NO
 897          /* Output       :       NO
 898          /* Description: 上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
 899                                                                          调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，Off
             -setFlag置1。
 900          /*---------------------------------------------------------------------------*/
 901          void GetCurrentOffset(void)
 902          {     
 903   1                SetBit(ADC_CR, ADCBSY);                                          // 使能ADC
 904   1                      
 905   1                      while(ReadBit(ADC_CR, ADCBSY));
 906   1                      
 907   1                      #if (Shunt_Resistor_Mode == Single_Resistor)                           // 29.2ms 单电阻模式，上电验证硬件电�
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 16  

             -��时，需观察mcCurOffset.IbusOffset是否为4096
 908   1                {
 909   2                              mcCurOffset.Iw_busOffsetSum+=((ADC4_DR& 0x0fff) << 3);
 910   2                              mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
 911   2                              mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
 912   2                      }
 913   1                      #elif (Shunt_Resistor_Mode == Double_Resistor)                             //44ms 双电阻模式，上电验证硬件电路�
             -�，需观察mcCurOffset.IaOffset、mcCurOffset.IbOffset是否为4096
                              {
                                      mcCurOffset.IuOffsetSum+=((ADC0_DR& 0x0fff) << 3);
                                      mcCurOffset.IuOffset = mcCurOffset.IuOffsetSum >> 4;
                                      mcCurOffset.IuOffsetSum -= mcCurOffset.IuOffset;
              
                                      mcCurOffset.IvOffsetSum+=((ADC1_DR& 0x0fff) << 3);
                                      mcCurOffset.IvOffset = mcCurOffset.IvOffsetSum >> 4;
                                      mcCurOffset.IvOffsetSum -= mcCurOffset.IvOffset;
                                      
                                      mcCurOffset.Iw_busOffsetSum+=((ADC4_DR& 0x0fff) << 3);
                                      mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
                                      mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
                              }
                              #elif (Shunt_Resistor_Mode == Three_Resistor)                         //58.2ms 三电阻模式，上电验证硬件电�
             -�时，需观察mcCurOffset.IaOffset、mcCurOffset.IbOffset、mcCurOffset.IcOffset是否为4096
                              {
                                      mcCurOffset.IuOffsetSum+=((ADC0_DR& 0x0fff) << 3);
                                      mcCurOffset.IuOffset = mcCurOffset.IuOffsetSum >> 4;
                                      mcCurOffset.IuOffsetSum -= mcCurOffset.IuOffset;
              
                                      mcCurOffset.IvOffsetSum+=((ADC1_DR& 0x0fff) << 3);
                                      mcCurOffset.IvOffset = mcCurOffset.IvOffsetSum >> 4;
                                      mcCurOffset.IvOffsetSum -= mcCurOffset.IvOffset;
              
                                      mcCurOffset.Iw_busOffsetSum+=((ADC4_DR& 0x0fff) << 3);
                                      mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
                                      mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;                        
                              }
                              #endif
 942   1      
 943   1                      mcCurOffset.OffsetCount++;
 944   1                      
 945   1                      if(mcCurOffset.OffsetCount>Calib_Time)
 946   1                      {
 947   2                              mcCurOffset.OffsetFlag=1;
 948   2          }
 949   1      }
 950          /*---------------------------------------------------------------------------*/
 951          /* Name         :       void Motor_Ready(void)
 952          /* Input        :       NO
 953          /* Output       :       NO
 954          /* Description: 上电时，关闭输出，先对硬件电路的电流进行采集，在FOC_Init中写入�
             -��应的校准寄存器中。
 955                          调试时，需观察mcCurOffset结构体中对应变量是否在范围内。
 956          /*---------------------------------------------------------------------------*/
 957          //void Motor_Ready(void)
 958          //{
 959          //   if(McStaSet.SetFlag.CalibFlag==0)
 960          //   {
 961          //     McStaSet.SetFlag.CalibFlag=1;
 962          //     ClrBit(DRV_CR, FOCEN);      // 关闭FOC
 963          //     MOE                      = 0;               // 关闭MOE
 964          //     SetBit(ADC_MASK_SYSC, CH7EN |CH4EN | CH3EN | CH2EN | CH1EN | CH0EN);// 开启ADC
 965          //     mcCurOffset.OffsetFlag=0;   // 开始电流采集
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/25/2018 17:39:39 PAGE 17  

 966          //   }
 967          //}
 968          /*---------------------------------------------------------------------------*/
 969          /* Name         :       void Motor_Init(void)
 970          /* Input        :       NO
 971          /* Output       :       NO
 972          /* Description: 对电机相关变量、PI进行初始化设置
 973          /*---------------------------------------------------------------------------*/
 974          void Motor_Init(void)
 975          {
 976   1      //   ClrBit(ADC_MASK_SYSC, CH4EN |  CH1EN | CH0EN);// 关闭软件电流采样的ADC
 977   1               VariablesPreInit();                           // 电机相关变量初始化
 978   1      //       PI_Init();                                                    // PI初始化
 979   1      }
 980          #endif


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   1944    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =     23    ----
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
   IDATA SIZE       =   ----    ----
   BIT SIZE         =   ----    ----
   EDATA SIZE       =   ----    ----
   HDATA SIZE       =   ----    ----
   XDATA CONST SIZE =   ----    ----
   FAR CONST SIZE   =   ----    ----
END OF MODULE INFORMATION.


C51 COMPILATION COMPLETE.  0 WARNING(S),  0 ERROR(S)
