C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE FOCCONTROLFUNCTION
OBJECT MODULE PLACED IN .\Output\FocControlFunction.obj
COMPILER INVOKED BY: C:\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             : 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;
*** ERROR C129 IN LINE 17 OF ..\User\Source\Function\FocControlFunction.c: missing ';' before 'xdata'
  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                  /*使能FOC*/
  28                  ClrBit(DRV_CR, FOCEN);
  29              _nop_();
  30                  SetBit(DRV_CR, FOCEN);
  31                  /*配置FOC寄存器*/
  32                  FOC_CR1                 = 0;                                                                                                            // 清零 FOC_CR1
  33                  FOC_CR2                 = 0;                                                                                                            // 清零 FOC_CR2
  34                  FOC_IDREF               = 0;                                                                                                            // 清零 Id
  35                  FOC_IQREF               = 0;                                                                                                            // 清零 Iq
  36          
  37                  FOC__THETA              = 0;                                                                                                            // 清零 角度
  38                  FOC_RTHEACC     = 0;                                                                                                            // 清零 爬坡函数的初始加速度
  39                  FOC__RTHESTEP   = 0;                                                                                                            //
  40                  FOC__RTHECNT    = 0;                                                                                                            //
  41                  FOC__THECOMP    = _Q15(0.0/180.0);                                                                                      // SMO 估算补偿角
  42                  FOC__THECOR     = 0x04;                                                                                                         // 误差角度补偿
  43          
  44                  /*电流环参数配置*/
  45                  FOC_DMAX                = DOUTMAX;
  46                  FOC_DMIN                = DOUTMIN;
  47          
  48                  FOC_QMAX                = QOUTMAX;
  49                  FOC_QMIN                = QOUTMIN;
  50          
  51                  /*位置估算参数配置*/
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 2   

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

                      #elif (Shunt_Resistor_Mode == Double_Resistor)                                  // double resistor sample
 113                  {
 114                          SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
 115          
 116                          FOC_TSMIN = PWM_DT_LOAD;                                                                                                                        // 死区补偿值
 117                          FOC_TRGDLY = 0x82;                                                                                                                                        // ADC采样的时刻，采样时刻在计数器零点附近，83�
             -�下降沿结束前3个clock采样Ia，与单电阻不同
 118                                                                                // 01为上升沿开始后第一个clock开始采�
             -��。根据实际情况调整。
 119                          FOC_TBLO=PWM_DLOWL_TIME;                              //下桥臂最小脉冲，保证采样
 120                          SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
 121          
 122                          /*五段式或七段式选择*/
 123                          #if (SVPMW_Mode == SVPWM_7_Segment)
 124                          {
 125                                  ClrBit(FOC_CR2,F5SEG);                                                                                                                  // 7段式
 126                          }
 127                          #elif (SVPMW_Mode == SVPWM_5_Segment)
                              {
                                      SetBit(FOC_CR2,F5SEG);                                                                                                                  // 7段式
                              }
                              #endif
 132          
 133                          #if (DouRes_Sample_Mode == DouRes_1_Cycle)
 134                          {
 135                                  ClrBit(FOC_CR2,DSS);                                                                                                                      // 7段式
 136                          }
 137                          #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
                              {
                                      SetBit(FOC_CR2,FOC_DSS);                                                                                                                // 7段式
                              }
                              #endif //end DouRes_Sample_Mode
 142                  }
 143                  /*三电阻采样*/
 144                  #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段式
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 4   

                              }
                              #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
                              {
                                      SetBit(FOC_CR2,DSS);                                                                                                                    // 7段式
                              }
                              #endif //end DouRes_Sample_Mode
                      }
                      #endif  //end Shunt_Resistor_Mode
 178          
 179          
 180                  /* 使能电流基准校正     */
 181                  #if (CalibENDIS == Enable)
                      {
                              if(mcCurOffset.OffsetFlag==1)
                              {
                                      #if (Shunt_Resistor_Mode == Single_Resistor)          // 单电阻校正
                                      {
                                              /*set ibus current sample offset*/
                                              SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                                              FOC_CSO = mcCurOffset.Iw_busOffset;                   // 写入Ibus的偏置
              
                                      }
                                      #elif (Shunt_Resistor_Mode == Double_Resistor)                  // 双电阻校正
                                      {
                                              /*set ia, ib current sample offset*/
                                              SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                                              FOC_CSO  = mcCurOffset.IuOffset;                  // 写入IA的偏置
              
                                              SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                                              FOC_CSO  = mcCurOffset.IvOffset;                        // 写入IB的偏置
              
                                      }
                                      #elif (Shunt_Resistor_Mode == Three_Resistor)         // 三电阻校正
                                      {
                                              /*set ibus current sample offset*/
                                              SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                                              FOC_CSO = mcCurOffset.IuOffset;                                                                         // 写入IA的偏置
              
                                              SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                                              FOC_CSO = mcCurOffset.IvOffset;                   // 写入IB的偏置
              
                                              SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                                              FOC_CSO = mcCurOffset.Iw_busOffset;                   // 写入IC的偏置
                                      }
                                      #endif  //end Shunt_Resistor_Mode
                              }
                      }
                      #endif  //end CalibENDIS
 218                  /*-------------------------------------------------------------------------------------------------
 219                  DRV_CTL：PWM来源选择
 220                  OCS = 0, DRV_COMR
 221                  OCS = 1, FOC/SVPWM/SPWM
 222                  -------------------------------------------------------------------------------------------------*/
 223                  /*计数器比较值来源FOC*/
 224                  SetBit(DRV_CR, OCS);
 225          }
 226          /*---------------------------------------------------------------------------*/
 227          /* Name         :       void Motor_Charge(void)
 228          /* Input        :       NO
 229          /* Output       :       NO
 230          /* Description: 预充电，当一直处于预充电状态下，不接电机，可用于验证IPM或者Mos
             -。
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 5   

 231          预充电分三步，第一步是对U相进行预充电，第二步是对U,V两相进行预充电;第三�
             -�是对U、V、W三相进行预充电。
 232          /*---------------------------------------------------------------------------*/
 233          void Motor_Charge(void)
 234          {
 235                  if(McStaSet.SetFlag.ChargeSetFlag==0)
 236                  {
 237                          McStaSet.SetFlag.ChargeSetFlag = 1;
 238                          #if (PWM_Level_Mode == Low_Level)        //用于測試6536
                              {
                                      SetBit(P1_OE, P10);
                                      GP10=1;
                              }
                              #endif
 244                          #if (IPMState == IPMtest)
                              {
                                      DRV_DR = 0.7*DRV_ARR;                                               // IPM 70% duty
                              }
                              #elif (IPMState == NormalRun)                 // 正常按电机状态机运行
 249                          {
 250                                  DRV_DR = 0.1*DRV_ARR;                                   //下桥臂10% duty
 251                          }
 252                          #endif
 253                          /*-------------------------------------------------------------------------------------------------
 254                          DRV_CTL：PWM来源选择
 255                          OCS = 0, DRV_COMR
 256                          OCS = 1, FOC/SVPWM/SPWM
 257                          -------------------------------------------------------------------------------------------------*/
 258                          ClrBit(DRV_CR, OCS);
 259                          mcFocCtrl.ChargeStep = 0;
 260                          mcFocCtrl.State_Count = Charge_Time;
 261                  }
 262                  if((mcFocCtrl.State_Count < Charge_Time)&&(mcFocCtrl.ChargeStep == 0))
 263                  {
 264                          mcFocCtrl.ChargeStep = 1;
 265                          #if (IPMState == IPMtest)
                              {
                                      DRV_CMR |= 0x03;                         // U相输出
                              }
                              #elif (IPMState == NormalRun)                     // 正常按电机状态机运行
 270                          {
 271                                  DRV_CMR |= 0x01;                         // U相下桥臂通
 272                          }
 273                          #endif
 274                          MOE = 1;
 275                  }
 276                  if(( mcFocCtrl.State_Count <= (Charge_Time<<1)/3)&&(mcFocCtrl.ChargeStep== 1))
 277                  {
 278                          mcFocCtrl.ChargeStep = 2;
 279                          #if (IPMState == IPMtest)
                              {
                                      DRV_CMR |= 0x0F;                         // U、V相输出
                              }
                              #elif (IPMState == NormalRun)                     // 正常按电机状态机运行
 284                          {
 285                                  DRV_CMR |= 0x04;                         // V相下桥臂导通
 286                          }
 287                          #endif
 288                  }
 289                  if((mcFocCtrl.State_Count <= Charge_Time/3)&&(mcFocCtrl.ChargeStep== 2))
 290                  {
 291                          mcFocCtrl.ChargeStep = 3;
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 6   

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

                                        FOC__ETHETA   = FOC__THETA;
                                      }
                                      #endif //end    EstimateAlgorithm
 356          
 357                            /*使能输出*/
 358                                  DRV_CMR |= 0x3F;                         // U、V、W相输出
 359                                  MOE = 1;
 360            }
 361          }
 362          
 363          /*---------------------------------------------------------------------------*/
 364          /* Name         :       void Motor_Open(void)
 365          /* Input        :       NO
 366          /* Output       :       NO
 367          /* Description: 开环启动的参数配置
 368          /*---------------------------------------------------------------------------*/
 369          void Motor_Open(void)
 370          {
 371                  static uint8 OpenRampCycles;
 372                  if(McStaSet.SetFlag.StartSetFlag==0)
 373                  {
 374                          McStaSet.SetFlag.StartSetFlag=1;
 375          
 376          
 377                          /****启动初始角度赋值**/
 378                          #if (PosCheckEnable)
 379                          {
 380                                  FOC__THETA      = mcFocCtrl.mcPosCheckAngle;// 有初始位置检测，则用初始位置角
 381                          }
 382                          #else
                              {
                                      FOC__THETA              = Align_Theta;            // 无初始位置检测，则用预定位角
                              }
                              #endif
 387                          /*********PLL或SMO**********/
 388                          #if (EstimateAlgorithm == SMO)
 389                          {
 390                                  FOC__ETHETA     = FOC__THETA-4915;
 391                          }
 392                          #elif (EstimateAlgorithm == PLL)
                              {
                                      FOC__ETHETA     = FOC__THETA;
                              }
                              #endif //end    EstimateAlgorithm
 397          
 398                          /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
 399                          FOC_IDREF = ID_Start_CURRENT;                         // D轴启动电流
 400                          mcFocCtrl.mcIqref= IQ_Start_CURRENT>>2;                       // Q轴启动电流
 401          
 402                          #if (FRDetectMethod==BEMFMethod)
                              {
                                      if(BEMFDetect.BEMFCCWFlag==1)
                                      {
                                              mcFocCtrl.mcIqref+=I_Value(0.95);                    //逆风时，加大启动电流
                                      }
                                      else if(BEMFDetect.BEMFCCWFlag==2)
                                      {
                                              mcFocCtrl.mcIqref+=I_Value(0.8);                    //逆风时，加大启动电流
                                      }
                              }
                              #endif
 414                          #if (FRDetectMethod==RSDMethod)
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 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          
 427                          FOC_DQKP = DQKPStart;
 428                          FOC_DQKI = DQKIStart;
 429          
 430          
 431                          FOC_EKP         = OBSW_KP_GAIN;
 432                          FOC_EKI         = OBSW_KI_GAIN;
 433          
 434          
 435                          /*启动方式选择*/
 436                          #if (Open_Start_Mode == Omega_Start)                  // Omega 启动
 437                          {
 438                                  FOC_EFREQACC    = Motor_Omega_Ramp_ACC;
 439                                  FOC_EFREQMIN    = Motor_Omega_Ramp_Min;
 440                                  FOC_EFREQHOLD = Motor_Omega_Ramp_End;
 441          
 442                                  SetBit(FOC_CR1,EFAE);                                                                                                                   // 估算器强制输出
 443                                  ClrBit(FOC_CR1,RFAE);                                                                                                                   // 禁止强拉
 444                                  SetBit(FOC_CR1,ANGM);                                                                                                                   // 估算模式
 445                          }
 446                          #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
 472                  }
 473                  /*不同启动方式下，切换到MCRUN状态*/
 474                  #if (Open_Start_Mode == Open_Start)                     //OPEN状态启动时拖动多次
                      {
                              if(OpenRampCycles<(MOTOR_OPEN_ACC_CYCLE-1))
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 9   

                              {
                                      if(!ReadBit(FOC_CR1,RFAE))
                                      {
                                              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 = 6000;
                              mcState = mcRun;
                      }
                      #elif (Open_Start_Mode == Omega_Start)
 499                  {
 500                          /*********PLL或SMO**********/
 501                          #if (EstimateAlgorithm == SMO)
 502                          {
 503                                  mcFocCtrl.State_Count = 6000;
 504                          }
 505                          #elif (EstimateAlgorithm == PLL)
                              {
                                      mcFocCtrl.State_Count = 2600;
                                      FOC_EKP = OBSW_KP_GAIN_RUN4;                            // 估算器里的PI的KP
                                      FOC_EKI = OBSW_KI_GAIN_RUN4;                                              // 估算器里的PI的KI
                              }
                              #endif //end    EstimateAlgorithm
 512                          mcState = mcRun;
 513                  }
 514                  #endif //end Open_Start_Mode
 515                  FOC_IQREF = mcFocCtrl.mcIqref;                              // Q轴启动电流
 516          }
 517          
 518          /*---------------------------------------------------------------------------*/
 519          /* Name         :       void Motor_Open(void)
 520          /* Input        :       NO
 521          /* Output       :       NO
 522          /* Description: 开环启动的参数配置
 523          /*---------------------------------------------------------------------------*/
 524          void Motor_PllStart(void)
 525          {
 526                  static uint8 OpenRampCycles;
 527                  if(TailWindDetect.PLLFlag==1)
 528                  {
 529                          TailWindDetect.PLLSpeed=FOC__EOME;
 530                          if(Abs_F16(TailWindDetect.PLLSpeed)<100)                //逆风状态下，速度低于100时，计数，达到一定
             -时间后，PLLFlag清零。
 531                          {
 532                                  TailWindDetect.PLLDetectCnt++;
 533                                  if(TailWindDetect.PLLDetectCnt>3000)            //防止估算错误
 534                                  {
 535                                          TailWindDetect.PLLFlag=0;
 536                                          TailWindDetect.PLLTheta=FOC__ETHETA;
 537                                          FOC_IQREF =0;
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 10  

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

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

 662                                  {
 663                                          TailWindDetectInit();
 664                                  }
 665                                  #endif
 666          
 667                          }
 668                          #if (FRDetectMethod==RSDMethod)
                              {
                                      RSDDealwith();
                              }
                              #elif (FRDetectMethod==BEMFMethod)
                              {
                                      BEMFDealwith();
                              }
                              #elif (FRDetectMethod==FOCMethod)
 677                          {
 678                                  FOCTailWindDealwith();
 679                          }
 680                          #endif
 681                  }
 682          }
 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                  MOE     = 0;
 693                  ClrBit(DRV_CR, FOCEN, 0);       //关闭FOC                                                                                                                                                                                                                     // disable FOC output and initial registe
             -r
 694                  mcState = mcInit;
 695          }
 696          
 697          /*-------------------------------------------------------------------------------------------------
 698                  Function Name : void MotorControlInit(void)
 699                  Description   : 控制变量初始化清零,包括保护参数的初始化、电机状态初始化
 700                  Input         : 输入说明（详细）
 701            Output                                :       输出说明（详细）
 702          -------------------------------------------------------------------------------------------------*/
 703          void MotorcontrolInit(void)
 704          {
 705            /***********保护******************/
 706                  memset(&mcFaultDect,0, sizeof(FaultVarible));                                                                                                                           // FaultVarible变量清零
 707            /******堵转保护*********/
 708          //      mcFaultDect.StallRecover          = 1000;//5s
 709          
 710            /******保护次数*********/
 711                  memset(&mcProtectTime,0, sizeof(ProtectVarible));                                                                                                               // ProtectVarible保护次数清零 
 712            /*******过流保护*****************/
 713                  memset(&mcCurVarible,0, sizeof(CurrentVarible));                                                                                                                // 电流保护的变量清零
 714          
 715            /*****顺逆风判断，FOCMethod不需要在此赋初值*******/
 716                  #if (FRDetectMethod==RSDMethod)
                      {
                 memset(&RSDDetect,0, sizeof(MotorRSDTypeDef));       //      RSDDetect所有变量清零
                      }
                      #elif (FRDetectMethod==BEMFMethod)
                      {
                 memset(&BEMFDetect,0, sizeof(BEMFDetect_TypeDef));           //      BEMFDetect所有变量清零
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 13  

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

 785                  memset(&mcFocCtrl,0, sizeof(FOCCTRL));                                                                                                                          // mcFocCtrl变量清零
 786              mcFocCtrl.OUTLoopCtrl.Close_loop_delay_time = SpeedloopDeley_Time;
 787          
 788            /*****顺逆风判断*******/
 789                  #if (FRDetectMethod==RSDMethod)
                      {
                        RSDDetect.RSDCCWTimes    = 0;                             //  CCW刹车次数清零
                      }
                      
                #endif
 795            /*****电机状态机时序变量***********/
 796            McStaSet.SetMode                   = 0;
 797          
 798                  /*****LED灯响应***/
 799                  memset(&mcLedDisplay,0, sizeof(MCLedDisplay));                                                                                          // mcLedDisplay变量清零
 800                  mcLedDisplay.Counttime            = 4999;
 801          
 802          
 803          }
 804          /*---------------------------------------------------------------------------*/
 805          /* Name         :       void GetCurrentOffset(void)
 806          /* Input        :       NO
 807          /* Output       :       NO
 808          /* Description: 上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
 809                                                                          调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，Off
             -setFlag置1。
 810          /*---------------------------------------------------------------------------*/
 811          void GetCurrentOffset(void)
 812          {
 813                  if(mcCurOffset.OffsetFlag==0)
 814            {      
 815                    SetBit(ADC_CR, ADCBSY);                                          // 使能ADC
 816                          while(ReadBit(ADC_CR, ADCBSY));
 817                          #if (Shunt_Resistor_Mode == Single_Resistor)                           // 29.2ms 单电阻模式，上电验证硬件电�
             -��时，需观察mcCurOffset.IbusOffset是否为4096
                        {
                                      mcCurOffset.Iw_busOffsetSum+=((ADC4_DR& 0x0fff) << 3);
                                      mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
                                      mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
                              }
                              #elif (Shunt_Resistor_Mode == Double_Resistor)                             //44ms 双电阻模式，上电验证硬件电路�
             -�，需观察mcCurOffset.IaOffset、mcCurOffset.IbOffset是否为4096
 824                          {
 825                                  mcCurOffset.IuOffsetSum+=((ADC0_DR& 0x0fff) << 3);
 826                                  mcCurOffset.IuOffset = mcCurOffset.IuOffsetSum >> 4;
 827                                  mcCurOffset.IuOffsetSum -= mcCurOffset.IuOffset;
 828          
 829                                  mcCurOffset.IvOffsetSum+=((ADC1_DR& 0x0fff) << 3);
 830                                  mcCurOffset.IvOffset = mcCurOffset.IvOffsetSum >> 4;
 831                                  mcCurOffset.IvOffsetSum -= mcCurOffset.IvOffset;
 832          
 833                          }
 834                          #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;
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 15  

              
                                      mcCurOffset.Iw_busOffsetSum+=((ADC4_DR& 0x0fff) << 3);
                                      mcCurOffset.Iw_busOffset = mcCurOffset.Iw_busOffsetSum >> 4;
                                      mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
                              }
                              #endif
 849          
 850                          mcCurOffset.OffsetCount++;
 851                          if(mcCurOffset.OffsetCount>Calib_Time)
 852                          {
 853                                  mcCurOffset.OffsetFlag=1;
 854              }
 855                  }
 856          }
 857          /*---------------------------------------------------------------------------*/
 858          /* Name         :       void Motor_Ready(void)
 859          /* Input        :       NO
 860          /* Output       :       NO
 861          /* Description: 上电时，关闭输出，先对硬件电路的电流进行采集，在FOC_Init中写入�
             -��应的校准寄存器中。
 862                          调试时，需观察mcCurOffset结构体中对应变量是否在范围内。
 863          /*---------------------------------------------------------------------------*/
 864          void Motor_Ready(void)
 865          {
 866             if(McStaSet.SetFlag.CalibFlag==0)
 867             {
 868               McStaSet.SetFlag.CalibFlag=1;
 869               ClrBit(DRV_CR, FOCEN);      // 关闭FOC
 870               MOE                        = 0;               // 关闭MOE
 871               SetBit(ADC_MASK_SYSC, CH7EN |CH4EN | CH3EN | CH2EN | CH1EN | CH0EN);// 开启ADC
 872          //     mcCurOffset.OffsetFlag=0;   // 开始电流采集
 873             }
 874          }
 875          /*---------------------------------------------------------------------------*/
 876          /* Name         :       void Motor_Init(void)
 877          /* Input        :       NO
 878          /* Output       :       NO
 879          /* Description: 对电机相关变量、PI进行初始化设置
 880          /*---------------------------------------------------------------------------*/
 881          void Motor_Init(void)
 882          {
 883          //   ClrBit(ADC_MASK_SYSC, CH4EN |  CH1EN | CH0EN);// 关闭软件电流采样的ADC
 884                   VariablesPreInit();                           // 电机相关变量初始化
 885                   PI_Init();                                                    // PI初始化
 886          }
 887          #endif
 888          
 889          
 890          void    FRControl(void)
 891          {
 892                  //正反转切换，存在如下情况：
 893                  //1、关机或错误状态下正反转切换，则直接改变当前旋转状态mcFRState.FR，下一次
             -启动则生效
 894                  //2、运行或启动状态，先关机并标记该状态，关机后进入Break状态直接改变旋转�
             -��态，
 895                  //且标记时重新开机并恢复转速
 896                  //3、正反转过程时关机，则保持关机状态，反转/正转切换后状态下一次开机后�
             -�效
 897                  
 898                  if(IRControl.FlagFR)
 899                  {
 900                          if((mcState == mcReady)||(mcState == mcFault))
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/26/2018 08:26:32 PAGE 16  

 901                          {
 902                                  //改变当前转向
 903                                  if(mcFRState.FR==CW)
 904                                  {       
 905                                          mcFRState.FR = CCW;
 906                                  }
 907                                  else
 908                                  {
 909                                          mcFRState.FR = CW;
 910                                  }                       
 911                                  
 912                                  //若为运行时正反转切换则恢复到切换前的转速并开机
 913                                  if(mcFRState.FlagFR)
 914                                  {
 915                                          mcState = mcInit;
 916          //                              IRControl.ONOFFStatus=1;
 917                                          mcSpeedRamp.FlagONOFF = 1;
 918                                          IRControl.TargetSpeed = mcFRState.OldTargetSpeed;
 919                                          IRControl.FlagSpeed = 1;        
 920                                          mcFRState.FlagFR = 0;
 921                                  }
 922                                  
 923                                  IRControl.FlagFR =0;
 924                          }
 925                          else if((mcState == mcRun)||(mcState == mcStart))
 926                          {
 927                                  //控制关机
 928          //                      IRControl.ONOFFStatus = 0;
 929          //                      IRControl.FlagONOFF = 1;
 930                                  mcSpeedRamp.FlagONOFF = 0;
 931                                  IRControl.TargetSpeed = 0;
 932                                  IRControl.FlagSpeed = 1;
 933                                  //运行状态下正反转切换置位标记
 934                                  mcFRState.FlagFR = 1;
 935                          }
 936                  }
 937                  
 938          }

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