C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE FOCCONTROLFUNCTION
OBJECT MODULE PLACED IN .\Output\FocControlFunction.obj
COMPILER INVOKED BY: D:\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) TABS
                    -(2) OBJECT(.\Output\FocControlFunction.obj)

line level    source

   1          /**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
   2          * File Name          : FocControl.c
   3          * Author             : Vina Peng,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          /*---------------------------------------------------------------------------*/
  20          /* Name   : void FOC_Init(void)
  21          /* Input  : NO
  22          /* Output : NO
  23          /* Description: mcInit状态下，对FOC的相关寄存器进行配置,先清理寄存器，后配置，�
             -�后使能
  24          /*---------------------------------------------------------------------------*/
  25          void FOC_Init(void)
  26          {
  27   1        /*使能FOC*/
  28   1        ClrBit(DRV_CR, FOCEN);
  29   1        SetBit(DRV_CR, FOCEN);
  30   1        /*配置FOC寄存器*/
  31   1        FOC_CR1       = 0;                                      // 清零 FOC_CR1
  32   1        FOC_CR2       = 0;                                      // 清零 FOC_CR2
  33   1        FOC_IDREF     = 0;                                      // 清零 Id
  34   1        FOC_IQREF     = 0;                                      // 清零 Iq
  35   1      
  36   1        FOC__THETA    = 0;                                      // 清零 角度
  37   1        FOC_RTHEACC   = 0;                                      // 清零 爬坡函数的初始加速度
  38   1        FOC__RTHESTEP   = 0;                                    //
  39   1        FOC__RTHECNT  = 0;                                      //
  40   1        FOC__THECOMP  = _Q15(0.0/180.0);                        // SMO 估算补偿角
  41   1        FOC__THECOR     = 0x04;                                 // 误差角度补偿
  42   1      
  43   1        /*电流环参数配置*/
  44   1        FOC_DMAX      = DOUTMAX;
  45   1        FOC_DMIN      = DOUTMIN;
  46   1      
  47   1        FOC_QMAX      = QOUTMAX;
  48   1        FOC_QMIN      = QOUTMIN;
  49   1      
  50   1        /*位置估算参数配置*/
  51   1        FOC_EK1       = OBS_K1T;
  52   1        FOC_EK2       = OBS_K2T;
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 2   

  53   1        FOC_EK3       = OBS_K3T;
  54   1        FOC_EK4       = OBS_K4T;
  55   1        FOC_FBASE     = OBS_FBASE;
  56   1        FOC_OMEKLPF   = SPEED_KLPF;
  57   1        FOC_EBMFK     = OBS_KLPF;
  58   1      
  59   1        FOC_TGLI      = PWM_TGLI_LOAD;
  60   1      
  61   1        /*********PLL或SMO**********/
  62   1        #if (EstimateAlgorithm == SMO)
  63   1        {
  64   2            ClrBit(FOC_CR2, ESEL);
  65   2            FOC_KSLIDE    = OBS_KSLIDE;
  66   2            FOC_EKLPFMIN  = OBS_EA_KS;
  67   2        }
  68   1        #elif (EstimateAlgorithm == PLL)
                {
                    SetBit(FOC_CR2, ESEL);
                    FOC_KSLIDE    = OBSE_PLLKP_GAIN;
                    FOC_EKLPFMIN  = OBSE_PLLKI_GAIN;
                }
                #endif //end SVPMW_Mode
  75   1      
  76   1        SetBit(FOC_CR1, SVPWMEN);                             // SVPWM模式
  77   1      
  78   1      //  /*正反转*/
  79   1      //  if(mcFRState.FR==1)                                 // 设置F/R
  80   1      //  {
  81   1      //    SetBit(DRV_CR,DRV_DDIR);
  82   1      //  }
  83   1      //  else
  84   1      //  {
  85   1      //    ClrBit(DRV_CR,DRV_DDIR);
  86   1      //  }
  87   1        #if ( IRMODE==1)
                {
                  SetBit(DRV_CR,DRV_DDIR);                               // 反转标志位
                }
                #endif //end IRMODE
  92   1      
  93   1        /**过调制**/
  94   1        #if (OverModulation == 1)
                {
                    SetBit(FOC_CR1,OVMDL);                          // 过调制
                }
                #endif //end OverModulation
  99   1      
 100   1        /*单电阻采样；需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式*/
 101   1        #if (Shunt_Resistor_Mode == Single_Resistor)
 102   1        {
 103   2          SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
 104   2          FOC_TSMIN = PWM_TS_LOAD;                              // 最小采样窗口
 105   2          FOC_TRGDLY =6;                                    // 采样时刻在中点，一般考虑开关噪声影响，会设�
             -��延迟；
 106   2                                                                // 0x0c表示延迟12个clock，提前用反码形
             -式，如0x84表示提前12个clock。
 107   2          ClrBit(FOC_CR2,F5SEG);                                // 7段式
 108   2          SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
 109   2        }
 110   1        /*双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia，配置81*/
 111   1        #elif (Shunt_Resistor_Mode == Double_Resistor)          // double resistor sample
                {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 3   

                  SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
              
                  FOC_TSMIN = PWM_DT_LOAD;                              // 死区补偿值
                  FOC_TRGDLY = 0x85;                                    // 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)
                  {
                    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)
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 4   

                  {
                    SetBit(FOC_CR2,DSS);                              // 7段式
                  }
                  #endif //end DouRes_Sample_Mode
                }
                #endif  //end Shunt_Resistor_Mode
 177   1      
 178   1      
 179   1        /* 使能电流基准校正 */
 180   1        #if (CalibENDIS == Enable)
 181   1        {
 182   2          if(mcCurOffset.OffsetFlag==1)
 183   2          {
 184   3            #if (Shunt_Resistor_Mode == Single_Resistor)        // 单电阻校正
 185   3            {
 186   4              /*set ibus current sample offset*/
 187   4              SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
 188   4              FOC_CSO = mcCurOffset.Iw_busOffset;               // 写入Ibus的偏置
 189   4      
 190   4            }
 191   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
 214   3          }
 215   2        }
 216   1        #endif  //end CalibENDIS
 217   1        /*-------------------------------------------------------------------------------------------------
 218   1        DRV_CTL：PWM来源选择
 219   1        OCS = 0, DRV_COMR
 220   1        OCS = 1, FOC/SVPWM/SPWM
 221   1        -------------------------------------------------------------------------------------------------*/
 222   1        /*计数器比较值来源FOC*/
 223   1        SetBit(DRV_CR, OCS);
 224   1      }
 225          /*---------------------------------------------------------------------------*/
 226          /* Name   : void Motor_Charge(void)
 227          /* Input  : NO
 228          /* Output : NO
 229          /* Description: 预充电，当一直处于预充电状态下，不接电机，可用于验证IPM或者Mos
             -。
 230          预充电分三步，第一步是对U相进行预充电，第二步是对U,V两相进行预充电;第三�
             -�是对U、V、W三相进行预充电。
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 5   

 231          /*---------------------------------------------------------------------------*/
 232          void Motor_Charge(void)
 233          {
 234   1        if(McStaSet.SetFlag.ChargeSetFlag==0)
 235   1        {
 236   2          McStaSet.SetFlag.ChargeSetFlag = 1;
 237   2          #if (PWM_Level_Mode == Low_Level)  //用于測試6536
                  {
                    SetBit(P1_OE, P10);
                    GP10=1;
                  }
                  #endif
 243   2          #if (IPMState == IPMtest)
                  {
                    DRV_DR = 0.7*DRV_ARR;               // IPM 70% duty
                  }
                  #elif (IPMState == NormalRun)         // 正常按电机状态机运行
 248   2          {
 249   3            DRV_DR = 0.1*DRV_ARR;               //下桥臂10% duty
 250   3          }
 251   2          #endif
 252   2          /*-------------------------------------------------------------------------------------------------
 253   2          DRV_CTL：PWM来源选择
 254   2          OCS = 0, DRV_COMR
 255   2          OCS = 1, FOC/SVPWM/SPWM
 256   2          -------------------------------------------------------------------------------------------------*/
 257   2          ClrBit(DRV_CR, OCS);
 258   2          mcFocCtrl.ChargeStep = 0;
 259   2        }
 260   1        if((mcFocCtrl.State_Count < Charge_Time)&&(mcFocCtrl.ChargeStep == 0))
 261   1        {
 262   2          mcFocCtrl.ChargeStep = 1;
 263   2          #if (IPMState == IPMtest)
                  {
                    DRV_CMR |= 0x03;                         // U相输出
                  }
                  #elif (IPMState == NormalRun)             // 正常按电机状态机运行
 268   2          {
 269   3            DRV_CMR |= 0x01;                         // U相下桥臂通
 270   3          }
 271   2          #endif
 272   2          MOE = 1;
 273   2        }
 274   1        if(( mcFocCtrl.State_Count <= (Charge_Time<<1)/3)&&(mcFocCtrl.ChargeStep== 1))
 275   1        {
 276   2          mcFocCtrl.ChargeStep = 2;
 277   2          #if (IPMState == IPMtest)
                  {
                    DRV_CMR |= 0x0F;                         // U、V相输出
                  }
                  #elif (IPMState == NormalRun)             // 正常按电机状态机运行
 282   2          {
 283   3            DRV_CMR |= 0x04;                         // V相下桥臂导通
 284   3          }
 285   2          #endif
 286   2        }
 287   1        if((mcFocCtrl.State_Count <= Charge_Time/3)&&(mcFocCtrl.ChargeStep== 2))
 288   1        {
 289   2          mcFocCtrl.ChargeStep = 3;
 290   2          #if (IPMState == IPMtest)
                  {
                  DRV_CMR |= 0x3F;                         // U、V、W相输出
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 6   

                  }
                  #elif (IPMState == NormalRun)             // 正常按电机状态机运行
 295   2          {
 296   3          DRV_CMR |= 0x10;                         // W相下桥臂导通
 297   3          }
 298   2          #endif
 299   2        }
 300   1      }
 301          /*---------------------------------------------------------------------------*/
 302          /* Name   : void Motor_Align(void)
 303          /* Input  : NO
 304          /* Output : NO
 305          /* Description: 预定位函数，当无逆风判断时，采用预定位固定初始位置;当有逆风�
             -�断时，采用预定位刹车
 306          /*---------------------------------------------------------------------------*/
 307          void Motor_Align(void)
 308          {
 309   1        if(McStaSet.SetFlag.AlignSetFlag==0)
 310   1        {
 311   2            McStaSet.SetFlag.AlignSetFlag=1;
 312   2      
 313   2            /*FOC初始化*/
 314   2            FOC_Init();
 315   2      
 316   2            /*配置预定位的电流、KP、KI*/
 317   2            FOC_IDREF = ID_Align_CURRENT;
 318   2            FOC_IQREF = IQ_Align_CURRENT;
 319   2      
 320   2            FOC_DQKP = DQKP_Alignment;
 321   2            FOC_DQKI = DQKI_Alignment;
 322   2      
 323   2            FOC_EKP   = OBSW_KP_GAIN;
 324   2            FOC_EKI   = OBSW_KI_GAIN;
 325   2      
 326   2            /*配置预定位角度*/
 327   2            #if (AlignTestMode==1)
                    {
                      FOC__THETA    = Align_Theta;
                    }
                    #else
 332   2            {
 333   3              #if (PosCheckEnable==1)
                      {
                        FOC__THETA  = mcFocCtrl.mcPosCheckAngle;
                      }
                      #else
 338   3              {
 339   4                FOC__THETA    =Align_Theta;
 340   4              }
 341   3              #endif  //end PosCheckEnable
 342   3            }
 343   2            #endif  //end AlignTestMode
 344   2                  /*********PLL或SMO**********/
 345   2            #if (EstimateAlgorithm == SMO)
 346   2            {
 347   3              FOC__ETHETA   = FOC__THETA-4096;
 348   3            }
 349   2            #elif (EstimateAlgorithm == PLL)
                    {
                      FOC__ETHETA   = FOC__THETA;
                    }
                    #endif //end  EstimateAlgorithm
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 7   

 354   2            TimeCnt=FOC__THETA;
 355   2      
 356   2            /*使能输出*/
 357   2            DRV_CMR |= 0x3F;                         // U、V、W相输出
 358   2            MOE = 1;
 359   2        }
 360   1      }
 361          
 362          /*---------------------------------------------------------------------------*/
 363          /* Name   : void Motor_Open(void)
 364          /* Input  : NO
 365          /* Output : NO
 366          /* Description: 开环启动的参数配置
 367          /*---------------------------------------------------------------------------*/
 368          void Motor_Open(void)
 369          {
 370   1        static uint8 OpenRampCycles;
 371   1        if(McStaSet.SetFlag.StartSetFlag==0)
 372   1        {
 373   2          McStaSet.SetFlag.StartSetFlag=1;
 374   2      
 375   2      
 376   2          /****启动初始角度赋值**/
 377   2          #if (PosCheckEnable)
                  {
                    FOC__THETA  = mcFocCtrl.mcPosCheckAngle;  // 有初始位置检测，则用初始位置角
                  }
                  #else
 382   2          {
 383   3            FOC__THETA  = Align_Theta;                // 无初始位置检测，则用预定位角
 384   3          }
 385   2          #endif
 386   2          /*********PLL或SMO**********/
 387   2          #if (EstimateAlgorithm == SMO)
 388   2          {
 389   3            FOC__ETHETA = FOC__THETA-4915;
 390   3          }
 391   2          #elif (EstimateAlgorithm == PLL)
                  {
                    FOC__ETHETA = FOC__THETA;
                  }
                  #endif //end  EstimateAlgorithm
 396   2      
 397   2          TimeCnt=FOC__ETHETA;              // 作用是什么？？？
 398   2          /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
 399   2          FOC_IDREF = ID_Start_CURRENT;                   // D轴启动电流
 400   2          mcFocCtrl.mcIqref= IQ_Start_CURRENT;        // Q轴启动电流
 401   2      
 402   2          #if (FRDetectMethod==BEMFMethod)
 403   2          {
 404   3            if(BEMFDetect.BEMFCCWFlag==1)
 405   3            {
 406   4              mcFocCtrl.mcIqref+=I_Value(0.95);       //逆风时，加大启动电流
 407   4            }
 408   3            else if(BEMFDetect.BEMFCCWFlag==2)
 409   3            {
 410   4              mcFocCtrl.mcIqref+=I_Value(0.8);        //逆风时，加大启动电流
 411   4            }
 412   3          }
 413   2          #endif
 414   2          #if (FRDetectMethod==RSDMethod)
                  {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 8   

                    if((RSDDetect.RSDCCWFlag==1)||(RSDDetect.RSDCCWTimes>=4))
                    {
                      mcFocCtrl.mcIqref+=I_Value(0.95);       //逆风时，加大启动电流
                    }
                    else if(RSDDetect.RSDCCWFlag==2)
                    {
                      mcFocCtrl.mcIqref+=I_Value(0.9);        //逆风时，加大启动电流
                    }
                  }
                  #endif
 426   2      
 427   2          FOC_DQKP  = DQKPStart;
 428   2          FOC_DQKI  = DQKIStart;
 429   2      
 430   2      
 431   2          FOC_EKP   = OBSW_KP_GAIN;
 432   2          FOC_EKI   = OBSW_KI_GAIN;
 433   2          /*启动方式选择*/
 434   2          #if (Open_Start_Mode == Omega_Start)            // Omega 启动
 435   2          {
 436   3            FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
 437   3            FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
 438   3            FOC_EFREQHOLD = Motor_Omega_Ramp_End;
 439   3      
 440   3            SetBit(FOC_CR1,EFAE);           // 估算器强制输出
 441   3            ClrBit(FOC_CR1,RFAE);           // 禁止强拉
 442   3            SetBit(FOC_CR1,ANGM);           // 估算模式
 443   3          }
 444   2          #elif (Open_Start_Mode == Open_Start)
                  {
                    FOC_RTHEACC   = Motor_Open_Ramp_ACC;    // 爬坡函数的初始加速度
                    FOC__RTHESTEP   = Motor_Open_Ramp_Min;    // 0.62 degree acce speed
                    RPDPara.ThetaGet=Motor_Open_Ramp_Min;
                    FOC__RTHECNT  = MOTOR_OPEN_ACC_CNT;   // acce time
              
                    ClrBit(FOC_CR1,EFAE);           // 估算器强制输出
                    SetBit(FOC_CR1,RFAE);           // 禁止强拉
                    ClrBit(FOC_CR1,ANGM);           // 估算模式
                  }
                  #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
 470   2        }
 471   1        /*不同启动方式下，切换到MCRUN状态*/
 472   1        #if (Open_Start_Mode == Open_Start)         //OPEN状态启动时拖动多次
                {
                  if(OpenRampCycles<(MOTOR_OPEN_ACC_CYCLE-1))
                  {
                    if(!ReadBit(FOC_CR1,RFAE))
                    {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 9   

                      SetBit(FOC_CR1,RFAE);
                      OpenRampCycles++;
                    }
                  }
                  else
                  {
                    mcFocCtrl.State_Count = 2;          //???这里为什么延时2ms
              
                    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)
 497   1        {
 498   2          /*********PLL或SMO**********/
 499   2          #if (EstimateAlgorithm == SMO)
 500   2          {
 501   3            mcFocCtrl.State_Count = 1100;
 502   3          }
 503   2          #elif (EstimateAlgorithm == PLL)
                  {
                    mcFocCtrl.State_Count = 1400;
                    FOC_EKP = OBSW_KP_GAIN_RUN1;              // 估算器里的PI的KP
                    FOC_EKI = OBSW_KI_GAIN_RUN1;        // 估算器里的PI的KI
                  }
                  #endif //end  EstimateAlgorithm
 510   2          mcState = mcRun;
 511   2        }
 512   1        #endif //end Open_Start_Mode
 513   1        FOC_IQREF = mcFocCtrl.mcIqref;                    // Q轴启动电流
 514   1      }
 515          
 516          /*---------------------------------------------------------------------------*/
 517          /* Name   : void Motor_Open(void)
 518          /* Input  : NO
 519          /* Output : NO
 520          /* Description: 开环启动的参数配置
 521          /*---------------------------------------------------------------------------*/
 522          void Motor_PllStart(void)
 523          {
 524   1        static uint8 OpenRampCycles;
 525   1        if(TailWindDetect.PLLFlag==1)
 526   1        {
 527   2          TailWindDetect.PLLSpeed=FOC__EOME;
 528   2              TailWindDetect.PLLSpeed=Abs_F16(TailWindDetect.PLLSpeed);
 529   2          if(Abs_F16(TailWindDetect.PLLSpeed)<350)    //逆风状态下，速度低于100时，计数，达到一定
             -时间后，PLLFlag清零。
 530   2          {
 531   3            TailWindDetect.PLLDetectCnt++;
 532   3            if(TailWindDetect.PLLDetectCnt>3000)    //防止估算错误
 533   3            {
 534   4              TailWindDetect.PLLFlag=0;
 535   4              TailWindDetect.PLLTheta=FOC__ETHETA;
 536   4      //        FOC_IQREF =0;
 537   4            }
 538   3          }
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 10  

 539   2          else
 540   2          {
 541   3            if(TailWindDetect.PLLDetectCnt>0)
 542   3            {
 543   4              TailWindDetect.PLLDetectCnt--;
 544   4            }
 545   3          }
 546   2        }
 547   1        if((McStaSet.SetFlag.StartSetFlag==0)&&(TailWindDetect.PLLFlag==0))
 548   1        {
 549   2          McStaSet.SetFlag.StartSetFlag=1;
 550   2          MOE = 0;
 551   2          FOC_Init();
 552   2          FOC__ETHETA   = TailWindDetect.PLLTheta;//+19668;//16668;//
 553   2      
 554   2          /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
 555   2          FOC_IDREF = ID_Start_CURRENT;                   // D轴启动电流
 556   2          FOC_IQREF = mcFocCtrl.mcIqref;                // Q轴启动电流
 557   2      
 558   2          FOC_DQKP = _Q12(2.0) ;//DQKPStart;
 559   2          FOC_DQKI = _Q15(0.16) ;//DQKIStart;
 560   2      //        FOC__UQ   =_Q15(0.3);
 561   2      
 562   2          /*启动方式选择*/
 563   2          #if (Open_Start_Mode == Omega_Start)            // Omega 启动
 564   2          {
 565   3            FOC_EFREQACC  = Motor_Omega_Ramp_ACC_Antiwind;
 566   3            FOC_EFREQMIN  = 3*Motor_Omega_Ramp_Min;
 567   3            FOC_EFREQHOLD   = Motor_Omega_Ramp_End;
 568   3      
 569   3            SetBit(FOC_CR1,EFAE);           // 禁止估算器强制输出
 570   3            ClrBit(FOC_CR1,RFAE);           // 使能强拉
 571   3            SetBit(FOC_CR1,ANGM);           // 估算模式
 572   3          }
 573   2          #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
              
                    ClrBit(FOC_CR1,EFAE);           // 禁止估算器强制输出
                    SetBit(FOC_CR1,RFAE);           // 使能强拉
                    ClrBit(FOC_CR1,ANGM);           // 估算模式
                  }
                  #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
 598   2          DRV_CMR |= 0x3F;                            // U、V、W相输出
 599   2          MOE = 1;
 600   2        }
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 11  

 601   1      
 602   1        if((McStaSet.SetFlag.StartSetFlag==1)&&(TailWindDetect.PLLFlag==0))
 603   1        {
 604   2          /*不同启动方式下，切换到MCRUN状态*/
 605   2          #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)
 629   2          {
 630   3            mcFocCtrl.State_Count = 2900;
 631   3            FOC_EKP = OBSW_KP_GAIN;             // 估算器里的PI的KP
 632   3            FOC_EKI = OBSW_KI_GAIN;       // 估算器里的PI的KI
 633   3            mcState = mcRun;
 634   3          }
 635   2          #endif //end Open_Start_Mode
 636   2        }
 637   1      
 638   1      }
 639          /*---------------------------------------------------------------------------*/
 640          /* Name   : void Motor_Align(void)
 641          /* Input  : NO
 642          /* Output : NO
 643          /* Description: 顺风逆风参数配置函数
 644          /*---------------------------------------------------------------------------*/
 645          void Motor_TailWind(void)
 646          {
 647   1        if(mcFocCtrl.State_Count == 0)
 648   1        {
 649   2          if(McStaSet.SetFlag.TailWindSetFlag  ==0)//初始化
 650   2          {
 651   3            McStaSet.SetFlag.TailWindSetFlag =1;
 652   3      
 653   3            #if (FRDetectMethod==RSDMethod)
                    {
                      RSDDetectInit();
                    }
                    #elif (FRDetectMethod==BEMFMethod)
 658   3            {
 659   4              BEMFDetectInit();
 660   4            }
 661   3            #elif (FRDetectMethod==FOCMethod)
                    {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 12  

                      TailWindDetectInit();
                    }
                    #endif
 666   3      
 667   3          }
 668   2          #if (FRDetectMethod==RSDMethod)
                  {
                    RSDDealwith();
                  }
                  #elif (FRDetectMethod==BEMFMethod)
 673   2          {
 674   3            BEMFDealwith();
 675   3          }
 676   2          #elif (FRDetectMethod==FOCMethod)
                  {
                    FOCTailWindDealwith();
                  }
                  #endif
 681   2        }
 682   1      }
 683          
 684          /*---------------------------------------------------------------------------*/
 685          /* Name   : void MC_Stop(void)
 686          /* Input  : NO
 687          /* Output : NO
 688          /* Description: inital motor control parameter
 689          /*---------------------------------------------------------------------------*/
 690          void MC_Stop(void)
 691          {
 692   1        MOE     = 0;
 693   1        ClrBit(DRV_CR, FOCEN, 0); //关闭FOC                                                     // disable FOC output and initial registe
             -r
 694   1        mcState = mcInit;
 695   1      }
 696          
 697          /*-------------------------------------------------------------------------------------------------
 698            Function Name : void MotorControlInit(void)
 699            Description   : 控制变量初始化清零,包括保护参数的初始化、电机状态初始化
 700            Input         : 输入说明（详细）
 701            Output        : 输出说明（详细）
 702          -------------------------------------------------------------------------------------------------*/
 703          void MotorcontrolInit(void)
 704          {
 705   1        /***********保护******************/
 706   1        memset(&mcFaultDect,0, sizeof(FaultVarible));                               // FaultVarible变量清零
 707   1        /******堵转保护*********/
 708   1      //  mcFaultDect.StallRecover          = 1000;//5s
 709   1      
 710   1        /******保护次数*********/
 711   1        memset(&mcProtectTime,0, sizeof(ProtectVarible));                           // ProtectVarible保护次数清零 
 712   1        /*******过流保护*****************/
 713   1        memset(&mcCurVarible,0, sizeof(CurrentVarible));                            // 电流保护的变量清零
 714   1      
 715   1        /*****顺逆风判断，FOCMethod不需要在此赋初值*******/
 716   1        #if (FRDetectMethod==RSDMethod)
                {
                 memset(&RSDDetect,0, sizeof(MotorRSDTypeDef)); //  RSDDetect所有变量清零
                }
                #elif (FRDetectMethod==BEMFMethod)
 721   1        {
 722   2         memset(&BEMFDetect,0, sizeof(BEMFDetect_TypeDef));   //  BEMFDetect所有变量清零
 723   2        } 
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 13  

 724   1        #endif
 725   1      
 726   1        /*******启停测试的参数***************/
 727   1        memset(&ONOFFTest,0, sizeof(ONVarible));
 728   1      
 729   1        /*****电机状态机时序变量***********/
 730   1        McStaSet.SetMode                   = 0;
 731   1      
 732   1        /*****电机目标方向**********/
 733   1        mcFRState.TargetFR                 = 0;
 734   1      
 735   1        /*****外部控制环*******/
 736   1        memset(&mcFocCtrl,0, sizeof(FOCCTRL));                                // mcFocCtrl变量清零
 737   1      
 738   1        /******ADC采样滤波值*********/
 739   1        memset(&AdcSampleValue,0, sizeof(ADCSample));                         // ADCSample变量清零
 740   1      
 741   1        /******电流偏置校准变量*****/
 742   1        memset(&mcCurOffset,0, sizeof(CurrentOffset));                        // mcCurOffset变量清零
 743   1        mcCurOffset.IuOffsetSum            = 16383;
 744   1        mcCurOffset.IvOffsetSum            = 16383;
 745   1        mcCurOffset.Iw_busOffsetSum        = 16383;
 746   1      
 747   1        /*****LED灯响应***/
 748   1        memset(&mcLedDisplay,0, sizeof(MCLedDisplay));                        // mcLedDisplay变量清零
 749   1        mcLedDisplay.Counttime            = 4999;
 750   1      
 751   1        /*****速度环的响应***/
 752   1        memset(&mcSpeedRamp,0, sizeof(MCRAMP));                               // mcSpeedRamp变量清零
 753   1      //  mcSpeedRamp.IncValue              = Motor_Speed_Inc;
 754   1      //  mcSpeedRamp.DecValue              = Motor_Speed_Dec;
 755   1        mcSpeedRamp.DelayPeriod           = 10;
 756   1      
 757   1        /*PWM调速变量*/
 758   1        memset(&mcPwmInput,0, sizeof(PWMINPUTCAL));                           // mcPwmInput变量清零
 759   1      
 760   1        /*串口变量*/
 761   1        memset(&Uart,0, sizeof(MCUART));                                      // MCUART变量清零
 762   1      
 763   1        /*睡眠模式*/  
 764   1        memset(&SleepSet,0, sizeof(SLEEPMODE));                                     // 睡眠模式清零
 765   1      
 766   1      
 767   1      }
 768          
 769          /*---------------------------------------------------------------------------*/
 770          /* Name   : void VariablesPreInit(void)
 771          /* Input  : NO
 772          /* Output : NO
 773          /* Description: 初始化电机参数
 774          /*---------------------------------------------------------------------------*/
 775          void VariablesPreInit(void)
 776          {
 777   1        /***********保护******************/
 778   1        mcFaultSource = 0;
 779   1        memset(&mcFaultDect,0, sizeof(FaultVarible));                               // FaultVarible变量清零
 780   1      
 781   1        /*****外部控制环*******/
 782   1        memset(&mcFocCtrl,0, sizeof(FOCCTRL));                                // mcFocCtrl变量清零
 783   1      
 784   1        /*****顺逆风判断*******/
 785   1        #if (FRDetectMethod==RSDMethod)
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 14  

                {
                  RSDDetect.RSDCCWTimes    = 0;                             //  CCW刹车次数清零
                }
                #elif (FRDetectMethod==BEMFMethod)
 790   1        {
 791   2        }
 792   1        #elif (FRDetectMethod==FOCMethod)
                {
                 memset(&TailWindDetect,0, sizeof(MotorTailWindTypeDef));   //  TailWindDetect所有变量清零
                }
                #endif
 797   1        /*****电机状态机时序变量***********/
 798   1        McStaSet.SetMode                   = 0;
 799   1      
 800   1        /*****LED灯响应***/
 801   1        memset(&mcLedDisplay,0, sizeof(MCLedDisplay));                        // mcLedDisplay变量清零
 802   1        mcLedDisplay.Counttime            = 4999;
 803   1      
 804   1      
 805   1      }
 806          /*---------------------------------------------------------------------------*/
 807          /* Name   : void GetCurrentOffset(void)
 808          /* Input  : NO
 809          /* Output : NO
 810          /* Description: 上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
 811                          调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，Off
             -setFlag置1。
 812          /*---------------------------------------------------------------------------*/
 813          void GetCurrentOffset(void)
 814          {
 815   1        if(mcCurOffset.OffsetFlag==0)
 816   1        {      
 817   2          SetBit(ADC_CR, ADCBSY);                              // 使能ADC
 818   2          while(ReadBit(ADC_CR, ADCBSY));
 819   2          #if (Shunt_Resistor_Mode == Single_Resistor)             // 29.2ms 单电阻模式，上电验证硬件电�
             -��时，需观察mcCurOffset.IbusOffset是否为4096
 820   2          {
 821   3            mcCurOffset.Iw_busOffsetSum+=((ADC4_DR& 0x0fff) << 3);
 822   3            mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
 823   3            mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
 824   3          }
 825   2          #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;
              
                  }
                  #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;
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 15  

                    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
 851   2      
 852   2          mcCurOffset.OffsetCount++;
 853   2          if(mcCurOffset.OffsetCount>Calib_Time)
 854   2          {
 855   3            mcCurOffset.OffsetFlag=1;
 856   3          }
 857   2        }
 858   1      }
 859          /*---------------------------------------------------------------------------*/
 860          /* Name   : void Motor_Ready(void)
 861          /* Input  : NO
 862          /* Output : NO
 863          /* Description: 上电时，关闭输出，先对硬件电路的电流进行采集，在FOC_Init中写入�
             -��应的校准寄存器中。
 864                          调试时，需观察mcCurOffset结构体中对应变量是否在范围内。
 865          /*---------------------------------------------------------------------------*/
 866          void Motor_Ready(void)
 867          {
 868   1         if(McStaSet.SetFlag.CalibFlag==0)
 869   1         {
 870   2           McStaSet.SetFlag.CalibFlag=1;
 871   2           ClrBit(DRV_CR, FOCEN);      // 关闭FOC
 872   2           MOE      = 0;               // 关闭MOE
 873   2           SetBit(ADC_MASK_SYSC, CH7EN |CH4EN | CH3EN | CH2EN | CH1EN | CH0EN);// 开启ADC
 874   2           mcCurOffset.OffsetFlag=0;   // 开始电流采集
 875   2         }
 876   1      }
 877          /*---------------------------------------------------------------------------*/
 878          /* Name   : void Motor_Init(void)
 879          /* Input  : NO
 880          /* Output : NO
 881          /* Description: 对电机相关变量、PI进行初始化设置
 882          /*---------------------------------------------------------------------------*/
 883          void Motor_Init(void)
 884          {
 885   1         ClrBit(ADC_MASK_SYSC, CH4EN |  CH1EN | CH0EN);// 关闭软件电流采样的ADC
 886   1         VariablesPreInit();                           // 电机相关变量初始化
 887   1         PI_Init();                                    // PI初始化
 888   1      }
 889          #endif


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   1797    ----
   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 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/06/2018 16:05:13 PAGE 16  


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