C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/12/2018 13:51:59 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) 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/12/2018 13:51:59 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 = 8;                                   // 采样时刻在中点，一般考虑开关噪声影响，会设
             -置延迟；
 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/12/2018 13:51:59 PAGE 3   

                  SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
              
                  FOC_TSMIN = PWM_DT_LOAD;                              // 死区补偿值
                  FOC_TRGDLY = 0x83;                                    // 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/12/2018 13:51:59 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/12/2018 13:51:59 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/12/2018 13:51:59 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/12/2018 13:51:59 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)
                  {
                    if(BEMFDetect.BEMFCCWFlag==1)
                    {
                      mcFocCtrl.mcIqref+=I_Value(0.95);       //逆风时，加大启动电流
                    }
                    else if(BEMFDetect.BEMFCCWFlag==2)
                    {
                      mcFocCtrl.mcIqref+=I_Value(0.8);        //逆风时，加大启动电流
                    }
                  }
                  #endif
 414   2          #if (FRDetectMethod==RSDMethod)
                  {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/12/2018 13:51:59 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      
 435   2          /*启动方式选择*/
 436   2          #if (Open_Start_Mode == Omega_Start)            // Omega 启动
 437   2          {
 438   3            FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
 439   3            FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
 440   3            FOC_EFREQHOLD = Motor_Omega_Ramp_End;
 441   3      
 442   3            SetBit(FOC_CR1,EFAE);           // 估算器强制输出
 443   3            ClrBit(FOC_CR1,RFAE);           // 禁止强拉
 444   3            SetBit(FOC_CR1,ANGM);           // 估算模式
 445   3          }
 446   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
 472   2        }
 473   1        /*不同启动方式下，切换到MCRUN状态*/
 474   1        #if (Open_Start_Mode == Open_Start)         //OPEN状态启动时拖动多次
                {
                  if(OpenRampCycles<(MOTOR_OPEN_ACC_CYCLE-1))
                  {
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/12/2018 13:51:59 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 = 2600;
                  mcState = mcRun;
                }
                #elif (Open_Start_Mode == Omega_Start)
 499   1        {
 500   2          /*********PLL或SMO**********/
 501   2          #if (EstimateAlgorithm == SMO)
 502   2          {
 503   3            mcFocCtrl.State_Count = 2100;
 504   3          }
 505   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
 512   2          mcState = mcRun;
 513   2        }
 514   1        #endif //end Open_Start_Mode
 515   1        FOC_IQREF = mcFocCtrl.mcIqref;                    // Q轴启动电流
 516   1      }
 517          
 518          /*---------------------------------------------------------------------------*/
 519          /* Name   : void Motor_Open(void)
 520          /* Input  : NO
 521          /* Output : NO
 522          /* Description: 开环启动的参数配置
 523          /*---------------------------------------------------------------------------*/
 524          void Motor_PllStart(void)
 525          {
 526   1        static uint8 OpenRampCycles;
 527   1        if(TailWindDetect.PLLFlag==1)
 528   1        {
 529   2          TailWindDetect.PLLSpeed=FOC__EOME;
 530   2              TailWindDetect.PLLSpeed=Abs_F16(TailWindDetect.PLLSpeed);
 531   2          if(Abs_F16(TailWindDetect.PLLSpeed)<350)    //逆风状态下，速度低于100时，计数，达到一定
             -时间后，PLLFlag清零。
 532   2          {
 533   3            TailWindDetect.PLLDetectCnt++;
 534   3            if(TailWindDetect.PLLDetectCnt>3000)    //防止估算错误
 535   3            {
 536   4              TailWindDetect.PLLFlag=0;
 537   4              TailWindDetect.PLLTheta=FOC__ETHETA;
 538   4      //        FOC_IQREF =0;
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/12/2018 13:51:59 PAGE 10  

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

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

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

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

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

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


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   1757    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =     23    ----
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
   IDATA SIZE       =   ----    ----
   BIT SIZE         =   ----    ----
   EDATA SIZE       =   ----    ----
   HDATA SIZE       =   ----    ----
   XDATA CONST SIZE =   ----    ----
C51 COMPILER V9.52.0.0   FOCCONTROLFUNCTION                                                09/12/2018 13:51:59 PAGE 16  

   FAR CONST SIZE   =   ----    ----
END OF MODULE INFORMATION.


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