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


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE ADDFUNCTION
OBJECT MODULE PLACED IN .\Output\AddFunction.obj
COMPILER INVOKED BY: F:\keil\C51\BIN\C51.EXE ..\User\Source\Application\AddFunction.c LARGE OMF2 WARNINGLEVEL(0) BROWSE 
                    -INCDIR(..\Library\FU68xx_Hardware_Driver\Include;..\User\Include) DEBUG PRINT(.\Listing\AddFunction.lst) OBJECT(.\Output
                    -\AddFunction.obj)

line level    source

   1          /**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
   2          * File Name          : AddFunction.c
   3          * Author             : Fortiortech  Appliction Team
   4          * Version            : V1.0
   5          * Date               : 2017-12-26
   6          * Description        : This file contains all the add function used for Motor Control.
   7          ***************************************************************************************************
   8          * All Rights Reserved
   9          **************************************************************************************************/
  10          
  11          
  12          /* Includes -------------------------------------------------------------------------------------*/
  13          // #include <AddFunction.h>
  14          #include <FU68xx_2.h>
  15          #include <Myproject.h>
  16          
  17          /* Private variables ---------------------------------------------------------*/
  18          FaultStateType                         mcFaultSource;
  19          PWMINPUTCAL        idata   mcPwmInput;
  20          FaultVarible       idata   mcFaultDect;
  21          MotorRSDTypeDef    idata   RSDDetect;
  22          CurrentVarible     idata   mcCurVarible;
  23          ProtectVarible     idata   mcProtectTime;
  24          Ramp_TypeDef       idata   SpeedRamp;
  25          
  26          FOCCTRL            xdata   mcFocCtrl;
  27          ADCSample          xdata   AdcSampleValue;
  28          ONVarible          xdata   ONOFFTest;
  29          MCLedDisplay       xdata   mcLedDisplay;
  30          MCRAMP             xdata   mcSpeedRamp;
  31          SLEEPMODE          xdata   SleepSet;
  32          MotorFRTypeDef           xdata   mcFRState;
  33          BreakDowmVarible   xdata     mcBreakDowm;
  34          int16                                            xdata   VSP;
  35          
  36          const int16  PowerGiven[10] ={0,500,1000,2200,2900,4300,5900,7300,7900,8550};
  37          
  38          uint8 OppoBrakeFlag = 0;//反角度刹车标志位,0为运行中，1为刹车中
  39          
  40          int16 RealAngleGivenToFoc = 0;
  41          
  42          extern  int Test2;
  43          
  44          extern char KeyBrake;
  45          int16 KeyBrakeRTheStep = 0;
  46          
  47          int16 KeyBrakeAngleCompensate = 0;
  48          
  49          int16 CCCW_Flag = 0;
  50          int tTest3 = 0;
  51          char BackSelfLock = 0;
  52          char OnSelfLock = 0;
  53          int16 RampStepInc = 0;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 2   

  54          extern int16 ReverseFlag;
  55          
  56          extern float g_fCarAngle;                                                                                                                                                                               //真实角度
  57          
  58          extern uint8 HallFRStatusTeat;
  59          
  60          uint8 SmoothFlag = 0;
  61          uint8 SmoothCnt = 0;
  62          /*---------------------------------------------------------------------------*/
  63          /* Name         :       int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  64          /* Input        :       INVlaue，OutLastValue
  65          /* Output       :       int16的变量
  66          /* Description: 滤波函数,用乘法器做的
  67          /*---------------------------------------------------------------------------*/
  68          int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  69          {
  70   1              int16 Result = 0;
  71   1              MDU_MA = (INVlaue-OutLastValue);
  72   1              MDU_MB = (int16)480;                                            /*写被乘数和乘数*/
  73   1      
  74   1              Result = MDU_MB;
  75   1              Result += OutLastValue;
  76   1              return(Result);
  77   1      }
  78          
  79          /*---------------------------------------------------------------------------*/
  80          /* Name         :       void FaultProcess(void)
  81          /* Input        :       NO
  82          /* Output       :       NO
  83          /* Description: 保护处理函数，关闭FOC输出，同时将状态变为mcFault
  84          /*---------------------------------------------------------------------------*/
  85          void FaultProcess(void)
  86          {
  87   1              MOE     = 0;
  88   1              ClrBit(DRV_CR, FOCEN);  //关闭FOC
  89   1              mcState = mcFault;
  90   1      
  91   1      }
  92          
  93          /*---------------------------------------------------------------------------*/
  94          /* Name         :       int16 Abs_F16(int16 value)
  95          /* Input        :       value
  96          /* Output       :       int16
  97          /* Description: 对变量取16位的绝对值
  98          /*---------------------------------------------------------------------------*/
  99          uint16 Abs_F16(int16 value)
 100          {
 101   1              if(value < 0)
 102   1              {
 103   2                      return (- value);
 104   2              }
 105   1              else
 106   1              {
 107   2                      return (value);
 108   2              }
 109   1      }
 110          /*---------------------------------------------------------------------------*/
 111          /* Name         :       int32 Abs_F32(int32 value)
 112          /* Input        :       value
 113          /* Output       :       int16
 114          /* Description: 对变量取16位的绝对值
 115          /*---------------------------------------------------------------------------*/
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 3   

 116          uint32 Abs_F32(int32 value)
 117          {
 118   1              if(value < 0)
 119   1              {
 120   2                      return (- value);
 121   2              }
 122   1              else
 123   1              {
 124   2                      return (value);
 125   2              }
 126   1      }
 127          /*---------------------------------------------------------------------------*/
 128          /* Name         :       void APP_DIV(void)
 129          /* Input        :       void
 130          /* Output       :       void
 131          /* Description: 将所有用到除法的地方，放在同一个中断，以避免中断串扰
 132          /*---------------------------------------------------------------------------*/
 133          void APP_DIV(void)
 134          {
 135   1          if( mcPwmInput.PWMDivFlag==1)  //启动除法器，避免与过调值中的除法冲突
 136   1          {
 137   2             mcPwmInput.PWMDuty = MDU_DIV_IDATA_U32(&mcPwmInput.pwm.PWMCompareAMP, &mcPwmInput.PWMARRUpdate);
 138   2             mcPwmInput.PWMDivFlag=0;
 139   2          }
 140   1          if( mcFocCtrl.ESDIVFlag==1)  //启动除法器，避免与过调值中的除法冲突
 141   1          {
 142   2             mcFocCtrl.SQUSpeedDIVEs = MDU_DIV_XDATA_U32(&mcFocCtrl.SQUSysSpeed,&mcFocCtrl.EsValue);
 143   2             mcFocCtrl.ESDIVFlag=0;
 144   2          }
 145   1      }
 146          /*---------------------------------------------------------------------------*/
 147          /* Name         :       void PWMInputCapture(void)
 148          /* Input        :       NO
 149          /* Output       :       NO
 150          /* Description: 输入PWM处理
 151          /*---------------------------------------------------------------------------*/
 152          void PWMInputCapture(void)
 153          {
 154   1        uint16 MotorControlVSP;
 155   1      
 156   1               if(mcPwmInput.PWMUpdateFlag==1)  // 有新的duty更新
 157   1               {
 158   2            if((Abs_F32(mcPwmInput.PWMCompare-mcPwmInput.PWMCompareOld)<50)// 两次比较值相近，减少读�
             -��率
 159   2              &&(Abs_F32(mcPwmInput.PWMARROld-mcPwmInput.PWMARR)<50)// 两次周期值相近，减少读错率
 160   2      //        &&((100<mcPwmInput.PWMARR)&&(mcPwmInput.PWMARR<6000))// 周期值在一定范围内才认为有�
             -��，即一定频率范围
 161   2              &&(mcPwmInput.PWMDivFlag==0))
 162   2              {
 163   3                mcPwmInput.PWMFlag=1;                               // PWMFlag置1期间，不读取TIM3__DR和T
             -IM3__ARR;，防止出错
 164   3                mcPwmInput.pwm.PWMCompareUpdate[0]=(mcPwmInput.PWMCompare>>1);// 对其乘以32768
 165   3                mcPwmInput.PWMARRUpdate=mcPwmInput.PWMARR;
 166   3                mcPwmInput.PWMDivFlag=1;                            // 启动除法
 167   3                mcPwmInput.PWMFlag=0;
 168   3              }
 169   2                                      
 170   2             if(mcPwmInput.PWMDivFlag==0)
 171   2              {
 172   3                if(mcPwmInput.PWMcnt<3)//2次求平均值
 173   3                {
 174   4                  mcPwmInput.PWMcnt++;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 4   

 175   4                  mcPwmInput.PWMVSum +=mcPwmInput.PWMDuty;
 176   4                }
 177   3                else
 178   3                {
 179   4                  MotorControlVSP= (mcPwmInput.PWMVSum >>1);//注意其有一个右移与前面的比较值标�
             -�化有关
 180   4                  mcPwmInput.PWMVSum=0;
 181   4                  mcPwmInput.PWMcnt =0;
 182   4                }
 183   3                                              
 184   3               MotorControlVSP = mcPwmInput.PWMDuty;
 185   3                                              
 186   3      //         if((MotorControlVSP > ONPWMDuty)&&(MotorControlVSP <=(OFFPWMDutyHigh+1)))
 187   3      //          {
 188   3      //            mcSpeedRamp.FlagONOFF = 1;
 189   3      //          }
 190   3      //         else if(MotorControlVSP < OFFPWMDuty)//||(MotorControlVSP >= OFFPWMDutyHigh))
 191   3      //          {
 192   3      //            mcSpeedRamp.FlagONOFF = 0;
 193   3      //          }
 194   3      
 195   3                //转速曲线计算
 196   3      //          if(mcSpeedRamp.FlagONOFF==1)
 197   3      //          {
 198   3      //            if(MotorControlVSP <= MINPWMDuty)
 199   3      //            {
 200   3      //              mcSpeedRamp.TargetValue = Motor_Min_Speed;
 201   3      //            }
 202   3      //            else if(MotorControlVSP < MAXPWMDuty)
 203   3      //            {
 204   3      //              mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(MotorControlVSP-MINPWMDuty);
 205   3      //            }
 206   3      //            else
 207   3      //            {
 208   3      //              mcSpeedRamp.TargetValue =       Motor_Max_Speed;
 209   3      //            }
 210   3      //          }
 211   3      //          else
 212   3      //          {
 213   3      //            mcSpeedRamp.TargetValue =0;
 214   3      //          }
 215   3              }
 216   2      
 217   2            mcPwmInput.PWMUpdateFlag =0;
 218   2            mcPwmInput.PWMCompareOld=mcPwmInput.PWMCompare;//将此次比较值赋值给上次比较值
 219   2            mcPwmInput.PWMARROld=mcPwmInput.PWMARR;//将此次周期值赋值给上次周期值
 220   2                }
 221   1      }
 222          /*****************************************************************************
 223           * Function:             void   Fault_OverVoltage(mcFaultVarible *h_Fault)
 224           * Description:  过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值�
             -�，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
 225                           同理，欠压保护。
 226                                 电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复
             -值以下时，计数器加一，超过200次后，恢复。根据档位信息来决定恢复到哪个状态。
 227           * Parameter:            mcFaultVarible *h_Fault
 228           * Return:                       no
 229           *****************************************************************************/
 230          void Fault_OverUnderVoltage(FaultVarible *h_Fault)
 231          {
 232   1              //过压保护
 233   1                      if(mcFaultSource == FaultNoSource)//程序无其他保护下
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 5   

 234   1                      {
 235   2                                      if(mcFocCtrl.mcADCVoltagebus > OVER_PROTECT_VALUE)       //母线电压大于过压保护值时，计数，
             -超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
 236   2                                      {
 237   3                                              h_Fault->OverVoltDetecCnt++;
 238   3                                              if(h_Fault->OverVoltDetecCnt > 20)//检测100ms
 239   3                                              {
 240   4                                                      h_Fault->OverVoltDetecCnt = 0;
 241   4                                                      mcFaultSource=FaultOverVoltage;
 242   4                                                      FaultProcess();
 243   4                                              }
 244   3                                      }
 245   2                                      else
 246   2                                      {
 247   3                                              if(h_Fault->OverVoltDetecCnt>0)
 248   3                                              {
 249   4                                                      h_Fault->OverVoltDetecCnt--;
 250   4                                              }
 251   3                                      }
 252   2      
 253   2                              //欠压保护
 254   2      
 255   2                                      if(mcFocCtrl.mcADCVoltagebus< UNDER_PROTECT_VALUE)
 256   2                                      {
 257   3                                              h_Fault->UnderVoltDetecCnt++;
 258   3      
 259   3                                              if(h_Fault->UnderVoltDetecCnt > 20)//检测100ms
 260   3                                              {
 261   4                                                      h_Fault->UnderVoltDetecCnt = 0;
 262   4                                                      mcFaultSource=FaultUnderVoltage;
 263   4                                                      FaultProcess();
 264   4                                              }
 265   3                                      }
 266   2                                      else
 267   2                                      {
 268   3                                              if(h_Fault->UnderVoltDetecCnt>0)
 269   3                                              {
 270   4                                                      h_Fault->UnderVoltDetecCnt--;
 271   4                                              }
 272   3                                      }
 273   2                      }
 274   1      
 275   1                      /*******过压欠压保护恢复*********/
 276   1                      #if (VoltageProtectRecoverEnble)
                              {
                                      if((mcState == mcFault) &&((mcFaultSource==FaultUnderVoltage)||(mcFaultSource==FaultOverVoltage)))
                                      {
                                              if((mcFocCtrl.mcADCVoltagebus< OVER_RECOVER_VALUE)&&(mcFocCtrl.mcADCVoltagebus> UNDER_RECOVER_VALUE))
                                              {
                                                      h_Fault->VoltRecoverCnt++;
                                                      if(h_Fault->VoltRecoverCnt>200)//连续检测1s，若正常则恢复
                                                      {
                                                              mcState = mcInit;
                                                              mcFaultSource=FaultNoSource;
                                                              h_Fault->VoltRecoverCnt = 0;
                                                      }
                                              }
                                              else
                                              {
                                                      h_Fault->VoltRecoverCnt = 0;
                                              }
                               }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 6   

                       }
                       #endif
 297   1      }
 298          
 299          /*****************************************************************************
 300           * Function:             void Fault_Overcurrent(CurrentVarible *h_Cur)
 301           * Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则O
             -verCurCnt加1。
 302                                                                           连续累加3次，判断为软件过流保护。执行时间约30.4us。
 303           * Parameter:            mcFaultVarible *h_Fault
 304           * Return:                       no
 305           *****************************************************************************/
 306          void Fault_Overcurrent(CurrentVarible *h_Cur)
 307          {
 308   1              if(mcState == mcRun)//||(mcState == mcStart))                                           // check over current in rum and open mode
 309   1              {
 310   2                      h_Cur->Abs_ia = Abs_F16(FOC__IA);
 311   2                      h_Cur->Abs_ib = Abs_F16(FOC__IB);
 312   2                      h_Cur->Abs_ic = Abs_F16(-FOC__IA-FOC__IB);
 313   2                      
 314   2      //              if(h_Cur->Abs_ia> h_Cur->Max_ia)                      // 此部分既用于软件过流保护，又用
             -于缺相保护
 315   2      //              {
 316   2      //                       h_Cur->Max_ia = h_Cur->Abs_ia;
 317   2      //              }
 318   2      //              if(h_Cur->Abs_ib > h_Cur->Max_ib)
 319   2      //              {
 320   2      //                       h_Cur->Max_ib = h_Cur->Abs_ib;
 321   2      //              }
 322   2      //              if(h_Cur->Abs_ic > h_Cur->Max_ic)
 323   2      //              {
 324   2      //                       h_Cur->Max_ic = h_Cur->Abs_ic;
 325   2      //              }
 326   2                      
 327   2      //              if((mcFocCtrl.mcADCProtectCurrentbus>=OverSoftCurrentValue)||(h_Cur->Abs_ib>=OverSoftCurrentValue)||(h
             -_Cur->Abs_ic>=OverSoftCurrentValue))
 328   2                      if(mcFocCtrl.mcADCProtectCurrentbus >= OverSoftCurrentValue)
 329   2                      {
 330   3                              h_Cur->OverCurCnt++;
 331   3                              if(h_Cur->OverCurCnt>=3)
 332   3                              {
 333   4      //                              h_Cur->Abs_ia=0;
 334   4      //                              h_Cur->Abs_ib=0;
 335   4      //                              h_Cur->Abs_ic=0;
 336   4                                      h_Cur->OverCurCnt=0;
 337   4                                mcFaultSource=FaultSoftOVCurrent;
 338   4                                FaultProcess();
 339   4                              }
 340   3                      }
 341   2          else
 342   2                      {
 343   3                              if(h_Cur->OverCurCnt>0)
 344   3                              {
 345   4                                      h_Cur->OverCurCnt--;
 346   4                              }
 347   3                      }
 348   2              }
 349   1      }
 350          
 351          /*****************************************************************************
 352           * Function:             void   Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
 353           * Description:  软硬件过流保护恢复
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 7   

 354           * Parameter:            mcFaultVarible *h_Fault
 355           * Return:                       no
 356           *****************************************************************************/
 357          void Fault_OverCurrentRecover(FaultVarible *h_Fault)
 358          {
 359   1              if((mcState == mcFault)&&((mcFaultSource==FaultSoftOVCurrent)||(mcFaultSource==FaultHardOVCurrent))&&(mcP
             -rotectTime.CurrentPretectTimes<5))
 360   1              {
 361   2                      h_Fault->CurrentRecoverCnt++;
 362   2                      if(h_Fault->CurrentRecoverCnt>=OverCurrentRecoverTime)//200*5=1s
 363   2                      {
 364   3                              h_Fault->CurrentRecoverCnt=0;
 365   3                              mcProtectTime.CurrentPretectTimes++;
 366   3                              mcState = mcInit;
 367   3                              mcFaultSource=FaultNoSource;
 368   3          }
 369   2        }
 370   1      }
 371          
 372          /*****************************************************************************
 373           * Function:             void   Fault_Start(mcFaultVarible *h_Fault)
 374           * Description:  启动保护函数，电机运行状态下，电机在前5s估算转速达到堵转保护
             -值或者5s后反电动势值太低(此方法未验证)
 375                            或4s内还在CtrlMode状态，即速度低于MOTOR_LOOP_RPM，程序判断为启动失�
             -��，电机停机。
 376                            当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进
             -入校准状态，等待重启。
 377           * Parameter:            mcFaultVarible *h_Fault
 378           * Return:                       no
 379           *****************************************************************************/
 380          //  void Fault_Start(FaultVarible *h_Fault)
 381          //  {
 382          //              /*******启动保护恢复*********/
 383          ////            h_Fault->mcEsValue = FOC__ESQU;
 384          
 385          //              if(mcState == mcRun)
 386          //              {
 387          //                      //方法一，5s内速度大于最大速度，同时反电动势值低于一定值
 388          //                      if(h_Fault->StartSpeedCnt<=1000)
 389          //                      {
 390          //                        h_Fault->StartSpeedCnt++;
 391          //                              if((mcFocCtrl.SpeedFlt > Motor_Max_Speed)&&(mcFocCtrl.EsValue<50))
 392          //                              {
 393          //                                      h_Fault->StartSpeedCnt = 0;
 394          //                                      mcFaultSource=FaultStart;
 395          //                                      FaultProcess();
 396          //                                      mcProtectTime.SecondStartTimes++;
 397          //                                      mcProtectTime.StartFlag  =  1;
 398          //        }
 399          //      }
 400          //     //方法二
 401          //                      if(h_Fault->StartEsCnt<=1200)//前6s，等待1.5s后，开始判断ES，如果超过一定次数，�
             -�失败
 402          //                      {
 403          //                              h_Fault->StartEsCnt++;
 404          //                              h_Fault->StartDelay++;
 405          //                              if(h_Fault->StartDelay>=300)                            // 1.5s
 406          //                              {
 407          //                                       h_Fault->StartDelay=300;
 408          //                                       if((mcFocCtrl.EsValue <5))//&&(mcFocCtrl.CtrlMode==0))
 409          //                                              {
 410          //                                                      h_Fault->StartESCount++;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 8   

 411          //                                                      if(h_Fault->StartESCount>=20)
 412          //                                                      {
 413          //                                                              mcFaultSource=FaultStart;
 414          //                                                              FaultProcess();
 415          //                                                              mcProtectTime.SecondStartTimes++;
 416          //                                                              h_Fault->StartDelay=0;
 417          //                                                              h_Fault->StartESCount=0;
 418          //                                                              mcProtectTime.StartFlag  =  2;
 419          //                                                      }
 420          //                                              }
 421          //            else
 422          //                                              {
 423          //                                                      if(h_Fault->StartESCount>0)
 424          //                                                              h_Fault->StartESCount--;
 425          //                                              }
 426          //                       }
 427          //               }
 428          //     else
 429          //     {
 430          //       h_Fault->StartESCount=0;
 431          //     }
 432          //                      //方法三，长时间在CtrlMode=0状态
 433          //                      if(mcFocCtrl.CtrlMode==0)         //
 434          //                      {
 435          //                              h_Fault->StartFocmode++;
 436          //                              if(h_Fault->StartFocmode>=400)
 437          //                              {
 438          //                                      h_Fault->StartFocmode=0;
 439          //                                      mcFaultSource=FaultStart;
 440          //                                      FaultProcess();
 441          //                                      mcProtectTime.SecondStartTimes++;
 442          //          mcProtectTime.StartFlag  =  3;
 443          //        }
 444          //                      }
 445          //        }
 446          
 447          //               if((mcFaultSource==FaultStart)&&(mcState == mcFault)&&(mcProtectTime.SecondStartTimes<=StartProtectRe
             -startTimes))
 448          //               {
 449          //                       mcFaultSource=FaultNoSource;
 450          //                       mcState = mcInit;
 451          //               }
 452          
 453          
 454          //      }
 455           /*****************************************************************************
 456           * Function:             void   Fault_Stall(mcFaultVarible *h_Fault)
 457           * Description:  堵转保护函数
 458           * Parameter:            mcFaultVarible *h_Fault
 459           * Return:                       no
 460           *****************************************************************************/
 461          //堵转保护
 462          void Fault_Stall(FaultVarible *h_Fault)
 463          {       
 464   1              if(!mcFocCtrl.LockedEnbleFlag)  //不锁机
 465   1              {
 466   2                      if((mcState == mcRun)||(mcState == mcStart))
 467   2                      {                                               
 468   3                              if(mcHall.HALLLockStatus)
 469   3                              {
 470   4                                      if(mcHall.mcStallTimeCnt < StallTime)
 471   4                                      {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 9   

 472   5                                              if(mcFocCtrl.mcIqref >= StallMINPWMDuty)
 473   5                                              {
 474   6                                                      mcFocCtrl.mcIqref -= 80;
 475   6      //                                              SpeedRamp.RampStep = mcFocCtrl.mcIqref;
 476   6                                                      
 477   6                                                      //FOC__UQ = SpeedRamp.RampStep;
 478   6      //                                              FOC_QMAX                = SpeedRamp.RampStep;
 479   6      //                                              FOC_QMIN                = SpeedRamp.RampStep;
 480   6                                              }       
 481   5                                      }
 482   4                                      else
 483   4                                      {
 484   5                                              mcHall.mcStallTimeCnt = StallTime;
 485   5                                              
 486   5                                              mcFaultSource=FaultStall;
 487   5      
 488   5                                              FaultProcess();         
 489   5                                      }
 490   4                              }                       
 491   3                      }
 492   2              }       
 493   1      
 494   1      //      if(mcState == mcRun)
 495   1      //      {
 496   1      //              if(h_Fault->StallDelayCnt <=1000)//5s
 497   1      //              {
 498   1      //                      h_Fault->StallDelayCnt ++;
 499   1      //              }
 500   1      //              else
 501   1      //              {
 502   1      //          //method 1，判断反电动势太小或当反电动势太小，转速太大
 503   1      //      if((&mcFocCtrl.EsValue< 10)||((FOC__EOME > _Q15(1600.0/MOTOR_SPEED_BASE))&&(mcFocCtrl.EsValue< 80)
             -))
 504   1      //                      {
 505   1      //                              h_Fault->StallDectEs++;
 506   1      //                              if(h_Fault->StallDectEs >= 10)
 507   1      //                              {
 508   1      //                                      h_Fault->StallDectEs=0;
 509   1      //                                      mcFaultSource=FaultStall;
 510   1      //                                      mcProtectTime.StallTimes++;
 511   1      //                                      FaultProcess();
 512   1      //                                      mcProtectTime.StallFlag  =  1;
 513   1      //                              }
 514   1      //                      }
 515   1      //                      else
 516   1      //                      {
 517   1      //        if(   h_Fault->StallDectEs>0)
 518   1      //                                h_Fault->StallDectEs--;
 519   1      //                      }
 520   1      //                      
 521   1      //                      //method 2，判断速度低于堵转最小值或者超过堵转最大值
 522   1      //                      if((mcFocCtrl.SpeedFlt<Motor_Stall_Min_Speed)||(mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
 523   1      //                      {
 524   1      //                              h_Fault->StallDectSpeed++;
 525   1      //                              if(h_Fault->StallDectSpeed>=8)
 526   1      //                              {
 527   1      //                                      h_Fault->StallDectSpeed=0;
 528   1      //                                      mcFaultSource=FaultStall;
 529   1      //                                      mcProtectTime.StallTimes++;
 530   1      //                                      FaultProcess();
 531   1      //                                      mcProtectTime.StallFlag =2;
 532   1      //                              }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 10  

 533   1      //                      }
 534   1      //                      else
 535   1      //                      {
 536   1      //        if(h_Fault->StallDectSpeed>0)
 537   1      //                              h_Fault->StallDectSpeed--;
 538   1      //                      }
 539   1      //    }
 540   1      //  }
 541   1      
 542   1      //               /*******堵转保护恢复*********/
 543   1      //        if((mcFaultSource==FaultStall)&&(mcState == mcFault)&&(mcProtectTime.StallTimes<=4))
 544   1      //        {
 545   1      ////          h_Fault->StallReCount++;
 546   1      ////          if(h_Fault->StallReCount>=StallRecoverTime)
 547   1      ////          {
 548   1      ////            h_Fault->StallReCount=16000;
 549   1      ////            mcFaultSource=FaultNoSource;
 550   1      ////            mcState = mcInit;
 551   1      ////          }
 552   1      //        }
 553   1      //        else
 554   1      //        {
 555   1      //          h_Fault->StallReCount=0;
 556   1      //        }
 557   1      //    }
 558   1      
 559   1      
 560   1      }
 561           /*****************************************************************************
 562           * Function:             void   Fault_phaseloss(mcFaultVarible *h_Fault)
 563           * Description:  缺相保护函数，当电机运行状态下，10ms取三相电流的最大值，
 564                                 1.5s判断各相电流最大值，若存在两相电流值大于一定值，而第三相�
             -�流值却非常小，则判断为缺相保护，电机停机；
 565           * Parameter:            mcFaultVarible *h_Fault
 566           * Return:                       no
 567           *****************************************************************************/
 568            void Fault_phaseloss(FaultVarible *h_Fault)
 569            {
 570   1                              if(mcState == mcRun)
 571   1                       {
 572   2                                      h_Fault->Lphasecnt++;
 573   2                                      if(h_Fault->Lphasecnt>100)//100*5=500ms
 574   2                                      {
 575   3                                               h_Fault->Lphasecnt=0;
 576   3                                               if(((mcCurVarible.Max_ia>(mcCurVarible.Max_ib*3))||(mcCurVarible.Max_ia>(mcCurVarible.Max_ic*3)))&&(
             -mcCurVarible.Max_ia>PhaseLossCurrentValue))
 577   3                                               {
 578   4                                                              h_Fault->AOpencnt++;
 579   4                                               }
 580   3                                               else
 581   3                                               {
 582   4                                                      if(h_Fault->AOpencnt>0)
 583   4                                                              h_Fault->AOpencnt --;
 584   4                                               }
 585   3                                               if(((mcCurVarible.Max_ib >(mcCurVarible.Max_ia*3))||(mcCurVarible.Max_ib >(mcCurVarible.Max_ic*3)))&
             -&(mcCurVarible.Max_ib >PhaseLossCurrentValue))
 586   3                                               {
 587   4                                                       h_Fault->BOpencnt++;
 588   4                                               }
 589   3                                              else
 590   3                                               {
 591   4                   if(h_Fault->BOpencnt>0)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 11  

 592   4                                                              h_Fault->BOpencnt --;
 593   4                                               }
 594   3                                               if(((mcCurVarible.Max_ic >(mcCurVarible.Max_ia*3))||(mcCurVarible.Max_ic >(mcCurVarible.Max_ib*3)))&
             -&(mcCurVarible.Max_ic >PhaseLossCurrentValue))
 595   3                                               {
 596   4                                                       h_Fault->COpencnt++;
 597   4                                               }
 598   3                                              else
 599   3                                               {
 600   4                   if(h_Fault->COpencnt>0)
 601   4                                                              h_Fault->COpencnt --;
 602   4                                               }
 603   3                                                      mcCurVarible.Max_ia = 0;
 604   3                                                      mcCurVarible.Max_ib = 0;
 605   3                                                      mcCurVarible.Max_ic = 0;
 606   3                                              if(h_Fault->AOpencnt > 1|| h_Fault->BOpencnt > 1 || h_Fault->COpencnt > 1)
 607   3                                               {
 608   4                                                              mcProtectTime.LossPHTimes++;
 609   4                                                              mcFaultSource=FaultLossPhase;
 610   4                                                              FaultProcess();
 611   4                                               }
 612   3                                      }
 613   2                              }
 614   1      
 615   1        }
 616          
 617          /*---------------------------------------------------------------------------*/
 618          /* Name         :       void Fault_Detection(void)
 619          /* Input        :       NO
 620          /* Output       :       NO
 621          /* Description: 保护函数，因保护的时间响应不会很高，采用分段处理，每5个定时�
             -�中断执行一次对应的保护
 622                                常见保护有过欠压、过温、堵转、启动、缺相等保护，调试时，可根�
             -��需求，一个个的调试加入。
 623          /*---------------------------------------------------------------------------*/
 624          void Fault_Detection(void)
 625          {
 626   1                      mcFaultDect.segment++;
 627   1                      if(mcFaultDect.segment>=5)
 628   1                      {
 629   2                              mcFaultDect.segment=0;
 630   2                      }
 631   1                      if(mcFaultDect.segment==0)
 632   1                              {
 633   2                                      if(CurrentRecoverEnable)   //过流保护恢复使能
 634   2                                      {
 635   3                                              Fault_OverCurrentRecover(&mcFaultDect);
 636   3                                      }
 637   2                              }
 638   1                      else if(mcFaultDect.segment==1)
 639   1                              {
 640   2              if(VoltageProtectEnable==1) //过压保护使能
 641   2                                      {
 642   3                      Fault_OverUnderVoltage(&mcFaultDect);
 643   3                                      }
 644   2                              }
 645   1                      else if(mcFaultDect.segment==2)
 646   1                              {
 647   2                                      if(StartProtectEnable==1)  //启动保护使能
 648   2                                      {
 649   3      //                                Fault_Start(&mcFaultDect);
 650   3                                      }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 12  

 651   2            }
 652   1                else if(mcFaultDect.segment==3)
 653   1                              {
 654   2                                      if(StallProtectEnable==1)  //堵转保护使能
 655   2                                      {
 656   3                                        Fault_Stall(&mcFaultDect);
 657   3                                      }
 658   2            }
 659   1                      else if(mcFaultDect.segment==4)
 660   1                              {
 661   2                                      if(PhaseLossProtectEnable==1)//缺相保护使能
 662   2                                      {
 663   3                                        Fault_phaseloss(&mcFaultDect);
 664   3                                      }
 665   2            }
 666   1                      else
 667   1                              {
 668   2      
 669   2            }
 670   1      }
 671          
 672          /*---------------------------------------------------------------------------*/
 673          /* Name         :       void ONOFF_Starttest(void)
 674          /* Input        :       NO
 675          /* Output       :       NO
 676          /* Description: 启动测试
 677          /*---------------------------------------------------------------------------*/
 678          //void ONOFF_Starttest(ONVarible  *h_test)
 679          //{
 680          //      if(h_test->ONOFF_Flag==1)
 681          //      {
 682          //               h_test->ON_Count++;
 683          //              if(h_test->ON_Count>StartON_Time)
 684          //              {
 685          //                      h_test->ON_Count=0;
 686          //                      h_test->ONOFF_Times++;
 687          //                      h_test->ONOFF_Flag=0;
 688          //                mcSpeedRamp.FlagONOFF = 0;
 689          //                      mcSpeedRamp.TargetValue = 0;
 690          //              }
 691          //  }
 692          //      else
 693          //      {
 694          //              if(mcState!=mcFault)
 695          //              {
 696          //                h_test->OFF_Count++;
 697          //                      if(h_test->OFF_Count>StartOFF_Time)
 698          //                      {
 699          //                              h_test->OFF_Count=0;
 700          //                              h_test->ONOFF_Flag=1;
 701          //                               mcSpeedRamp.FlagONOFF = 1;
 702          //                               mcSpeedRamp.TargetValue = Motor_Min_Speed;
 703          //                      }
 704          //        }
 705          //  }
 706          //}
 707          
 708          /*---------------------------------------------------------------------------*/
 709          /* Name         :       void Speed_response(void)
 710          /* Input        :       NO
 711          /* Output       :       NO
 712          /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 13  

             -、恒功率控制
 713          /*---------------------------------------------------------------------------*/
 714          void Speed_response(void)
 715          {
 716   1              static uint16  VOP_K = 0;
 717   1              static uint16 SPEED_MaxPWMDuty = 0;
 718   1      
 719   1              static uint8 SpeedLimit1IOCnt = 0;
 720   1              static uint8 SpeedLimit2IOCnt = 0;
 721   1              static uint8 SpeedLimit3IOCnt = 0;
 722   1      
 723   1              static uint8 SoftStartIOCnt = 0;
 724   1      
 725   1              static uint8 SlowStopDutyFlag = 0;
 726   1              
 727   1                                                      if(mcFocCtrl.mcSpeedVSP <= _Q15(ProtecMinPWMDuty))
 728   1                                                      {
 729   2                                                              mcFocCtrl.mcSpeedVSP = _Q15(ProtecMinPWMDuty);
 730   2                                                      }
 731   1                                                      else if(mcFocCtrl.mcSpeedVSP <= _Q15(ONPWMDuty))
 732   1                                                      {
 733   2                                                              mcFocCtrl.OpenDutyCycle = (mcFocCtrl.mcSpeedVSP - _Q15(ONPWMDuty));//MinPWMDuty;
 734   2                                                              //mcFocCtrl.RMSLimitTargetCurrent = IQ_RUN_MIN_RMS_CURRENT;
 735   2                                                      }
 736   1                                                      else if(mcFocCtrl.mcSpeedVSP <= _Q15(ProtecMaxPWMDuty))
 737   1                                                      {
 738   2                                                              //mcFocCtrl.TempOpenDutyCycle = (mcFocCtrl.mcSpeedVSP - _Q15(ONPWMDuty));
 739   2                                                              
 740   2                                                              //mcFocCtrl.TempOpenDutyCycle = mcFocCtrl.TempOpenDutyCycle >>6;
 741   2                                                                                                              
 742   2                                                              //mcFocCtrl.OpenDutyCycle = MinPWMDuty + VOP_K * mcFocCtrl.TempOpenDutyCycle;
 743   2                                                              mcFocCtrl.OpenDutyCycle = (mcFocCtrl.mcSpeedVSP - _Q15(ONPWMDuty));                                                                                                     
 744   2                                                              //mcFocCtrl.RMSLimitTargetCurrent = IQ_RUN_MIN_RMS_CURRENT+ IQ_RMSlIMIT_K * (mcFocCtrl.mcSpeedVSP-_
             -Q15(ONPWMDuty));
 745   2                                                      }
 746   1                                                      else
 747   1                                                      {
 748   2                                                              mcFocCtrl.mcSpeedVSP = _Q15(ProtecMaxPWMDuty);
 749   2                                                      }
 750   1                                                                              
 751   1                                                      
 752   1      //                                              RampStepInc = 80;
 753   1                                      if(SpeedRamp.RampStep <= mcFocCtrl.OpenDutyCycle)
 754   1                                      {
 755   2                                              if((SpeedRamp.RampStep + RampStepInc) <= mcFocCtrl.OpenDutyCycle)
 756   2                                              {
 757   3                                                      SpeedRamp.RampStep += RampStepInc;
 758   3                                              }
 759   2                                              else
 760   2                                              {
 761   3                                                      SpeedRamp.RampStep = mcFocCtrl.OpenDutyCycle;
 762   3                                              }
 763   2                                      }
 764   1                                      else
 765   1                                      {
 766   2                                              if((SpeedRamp.RampStep - RampStepInc) > mcFocCtrl.OpenDutyCycle)
 767   2                                              {
 768   3                                                      SpeedRamp.RampStep -= RampStepInc;
 769   3                                              }
 770   2                                              else
 771   2                                              {
 772   3                                                      SpeedRamp.RampStep = mcFocCtrl.OpenDutyCycle;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 14  

 773   3                                              }
 774   2                                      }
 775   1                                      
 776   1                                      
 777   1                                      //SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle); 
 778   1                                      
 779   1      //                              if(OppoBrakeFlag)
 780   1      //                              {
 781   1      //                                      mcFocCtrl.OpenDutyCycle = -1;
 782   1      //                                      SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle);
 783   1      //                                      
 784   1      //                                      if(mcHall.HallSpeed < 1000)
 785   1      //                                      {
 786   1      //                                              OppoBrakeFlag = 0;
 787   1      //                                              mcState = mcStop;
 788   1      //                                      }
 789   1      //                              }
 790   1      //                              if(KeyBrake)
 791   1      //                              {
 792   1      //                                      SpeedRamp.RampStep += 150;
 793   1      //                                      if(SpeedRamp.RampStep >= 16380)
 794   1      //                                      {
 795   1      //                                              SpeedRamp.RampStep  = 16380;
 796   1      //                                      }
 797   1      //                              }
 798   1      
 799   1      //                                      if(ReverseFlag)
 800   1      //                                      {
 801   1                                                      FOC__UQ = SpeedRamp.RampStep * 2;
 802   1      //                                      }
 803   1      //                              FOC_QMAX                = SpeedRamp.RampStep;
 804   1      //                              FOC_QMIN                = SpeedRamp.RampStep;
 805   1      //                        FOC_IQREF = SpeedRamp.RampStep/50;                                                                                                            
 806   1      
 807   1              }
 808          
 809          
 810          /*---------------------------------------------------------------------------*/
 811          /* Name         :       void FGOutput(void)
 812          /* Input        :       NO
 813          /* Output       :       NO
 814          /* Description: FG信号输出
 815          /*---------------------------------------------------------------------------*/
 816          void FGOutput(void)
 817          {//FG可用中断FGIF进行判断
 818   1              if(mcState==mcRun)
 819   1              {
 820   2                      if((FOC__THETA>=0)&&(FOC__THETA<32768))//0-180
 821   2                      {
 822   3                              ResetFGPin;
 823   3                      }
 824   2                      else if((FOC__THETA>=32768)&&(FOC__THETA<65536))//180-360
 825   2                      {
 826   3                              SetFGPin;
 827   3                      }
 828   2        }
 829   1              else if(mcState == mcFault)
 830   1              {
 831   2                              SetFGPin;
 832   2              }
 833   1              else
 834   1              {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 15  

 835   2                              ResetFGPin;
 836   2              }
 837   1      }
 838          /*---------------------------------------------------------------------------*/
 839          /* Name         :        uint16 SoftLPF(uint16 Xn1, uint16 Xn0, uint16 K)
 840          /* Input        :       uint16 Xn1, uint16 Xn0, uint16 K
 841          /* Output       :       uint16
 842          /* Description: 软件低通滤波
 843          /*---------------------------------------------------------------------------*/
 844           int16 SoftLPF(int16 Xn1, int16 Xn0, int16 K)
 845           {
 846   1              int16 Temp16 = 0;
 847   1              int32 Temp32 = 0;
 848   1      
 849   1              Temp32 = (((int32)Xn1 - (int32)Xn0) * (int32)K) >> 15;
 850   1              Temp16 = Xn0 + (int16)Temp32;
 851   1              return Temp16;
 852   1       }
 853          
 854          
 855          /*---------------------------------------------------------------------------*/
 856          /* Name         :       void LED_Display(void)
 857          /* Input        :       NO
 858          /* Output       :       NO
 859          /* Description: LED灯显示
 860          /*---------------------------------------------------------------------------*/
 861          void LED_Display(void)
 862          {
 863   1              switch(mcFaultSource)
 864   1              {
 865   2                      case FaultNoSource:
 866   2                          ResetLEDPin;                //低电平点亮，高电平灭
 867   2                        break;
 868   2                case FaultHardOVCurrent:
 869   2                                          Led_OnOff(&mcLedDisplay,1);
 870   2                                        break;
 871   2                                      case FaultSoftOVCurrent:
 872   2                                                Led_OnOff(&mcLedDisplay,2);
 873   2                                              break;
 874   2                      case FaultUnderVoltage:
 875   2                  Led_OnOff(&mcLedDisplay,3);
 876   2                              break;
 877   2                      case FaultOverVoltage:
 878   2                  Led_OnOff(&mcLedDisplay,4);
 879   2                              break;
 880   2                      case FaultLossPhase:
 881   2                                                Led_OnOff(&mcLedDisplay,5);
 882   2                                              break;
 883   2                                      case FaultStall:
 884   2                                                Led_OnOff(&mcLedDisplay,6);
 885   2                                        break;
 886   2                      case  FaultStart:
 887   2                  Led_OnOff(&mcLedDisplay,7);
 888   2                              break;
 889   2                                      case  FaultOverwind:
 890   2                                                Led_OnOff(&mcLedDisplay,8);
 891   2                                              break;
 892   2                      default:
 893   2                              break;
 894   2        }
 895   1      
 896   1      }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 16  

 897          
 898            //LED灯的闪烁
 899            void Led_OnOff(MCLedDisplay *hLedDisplay,uint8 htime)
 900            {
 901   1                      hLedDisplay->LedCount++;
 902   1                if(hLedDisplay->LedCount<hLedDisplay->Counttime)
 903   1                 {
 904   2                          if(hLedDisplay->Count<200)
 905   2                    {
 906   3                                       hLedDisplay->Count++;
 907   3      
 908   3                    }
 909   2                                      else if((hLedDisplay->Count>=200)&&(hLedDisplay->Count<201))
 910   2                                      {
 911   3                                              hLedDisplay->Count=0;
 912   3                                        LEDPinONOFF;
 913   3                                              hLedDisplay->LedTimCot++;
 914   3                                      }
 915   2                                      else
 916   2                                              ;
 917   2                                      if(hLedDisplay->LedTimCot>=2*htime)
 918   2                              {
 919   3                                              hLedDisplay->Count=202;
 920   3                                        SetLEDPin;
 921   3                                      }
 922   2            }
 923   1                  else if(hLedDisplay->LedCount>=hLedDisplay->Counttime)
 924   1                  {
 925   2                   hLedDisplay->LedCount=0;
 926   2                   hLedDisplay->LedTimCot=0;
 927   2                   hLedDisplay->Count=0;
 928   2                  }
 929   1       }
 930           
 931          /*---------------------------------------------------------------------------*/
 932          /* Name         :       void mc_ramp(void)
 933          /* Input        :       hTarget,MC_RAMP *hSpeedramp
 934          /* Output       :       NO
 935          /* Description:
 936          /*---------------------------------------------------------------------------*/
 937          
 938          //void mc_ramp(MCRAMP *hSpeedramp)
 939          //{
 940          //        if( --hSpeedramp->DelayCount < 0)
 941          //              {
 942          //                              hSpeedramp->DelayCount = hSpeedramp->DelayPeriod;
 943          
 944          //                              if (hSpeedramp->ActualValue < hSpeedramp->TargetValue)
 945          //                              {
 946          //                                              if(hSpeedramp->ActualValue + Motor_Speed_Inc < hSpeedramp->TargetValue)
 947          //                                              {
 948          //                                                              hSpeedramp->ActualValue += Motor_Speed_Inc;
 949          //                                              }
 950          //                                              else
 951          //                                              {
 952          //                                                              hSpeedramp->ActualValue = hSpeedramp->TargetValue;
 953          //                                              }
 954          //                              }
 955          //                              else
 956          //                              {
 957          //                                              if(hSpeedramp->ActualValue - Motor_Speed_Dec > hSpeedramp->TargetValue)
 958          //                                              {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 17  

 959          
 960          //                                                              hSpeedramp->ActualValue -= Motor_Speed_Dec;
 961          //                                              }
 962          //                                              else
 963          //                                              {
 964          //                                                              hSpeedramp->ActualValue = hSpeedramp->TargetValue;
 965          //                                              }
 966          //                              }
 967          //              }
 968          //}
 969          
 970          //void HW_Speed_PI(void)
 971          //{
 972          ////            PI_EK =  mcSpeedRamp.ActualValue- mcFocCtrl.Powerlpf;   //给定转速与实际转速之差，可区�
             -�恒功率或恒转速，或恒电压
 973          //              PI_EK =  mcSpeedRamp.ActualValue- FOC__EOME;    //给定转速与实际转速之差，直接减FOC__EOME�
             -��风险
 974          //              PI_LPF_CR |= 0x02;                                                                      // Start PI
 975          //              _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
 976          ////      mcFocCtrl.mcIqref= PI_UK;//可区分限功率与不限功率
 977          ////            FOC_IQREF =mcFocCtrl.mcIqref;
 978          //              PI_UK+=(SKP/4096 +1);
 979          //              FOC_IQREF= PI_UK;
 980          
 981          //}
 982          
 983          /*-------------------------------------------------------------------------------------------------
 984                  Function Name : int16 HW_Genal_PI(int16 Xn1, int16 Yn0, int16 Xn2)
 985                  U(K) = U(k-1) + Kp*(E(k)-E(k-1)) + Ki*E(k) ----- (Uk_min < U(k) < Uk_max)b
 986                  Description   : PI控制
 987                  Input         : Xn1--E(K-1)
 988                                  Yn0--U(K-1)
 989                                                  Xn2--E(K)
 990                  Output            :     PI_UK--当前PI输出值,执行时间为3.8us
 991          -------------------------------------------------------------------------------------------------*/
 992          //int16 HW_Genal_PI(int16 Xn1, int16 Yn0, int16 Xn2)
 993          //{
 994          //              PI_KP = SKP;
 995          //              PI_KI = SKI;
 996          //              PI_UKMAX = SOUTMAX;
 997          //              PI_UKMIN = SOUTMIN;
 998          //              PI_EK =  Xn1;             //初始化E(K-1)
 999          //              PI_LPF_CR |= 0x02;        // Start PI
1000          //              _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1001          //              PI_UK =  Yn0;           //初始化U(K-1)
1002          //              PI_EK =  Xn2;         //填入EK
1003          //              PI_LPF_CR |= 0x02;        // Start PI
1004          //              _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1005          //              PI_UK+=(SKP/4096 +1);
1006          //              return PI_UK;
1007          //}
1008          /*-------------------------------------------------------------------------------------------------
1009                  Function Name : int16 HW_One_PI(int16 Xn1, int16 Yn0, int16 Xn2)
1010                  Description   : PI控制
1011                  Input         : Xn1--E(K)
1012                  Output            :     PI_UK--当前PI输出值,执行时间us
1013          -------------------------------------------------------------------------------------------------*/
1014          //int16 HW_One_PI(int16 Xn1)
1015          //{
1016          //        PI_EK =  Xn1;       //填入EK
1017          //        PI_LPF_CR |= 0x02;      // Start PI
1018          //        _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 18  

1019          //        PI_UK+=(SKP/4096 +1);
1020          //        return PI_UK;
1021          //}
1022          
1023          /*-------------------------------------------------------------------------------------------------
1024                  Function Name : int16 LPF(int16 Xn1, int16 Xn0, int8 K)
1025                  Description   : LFP控制
1026                  Input         : Xn1--当前输入值
1027                                  Xn0--上一次滤波输出值
1028                                                                                  K--LPF滤波系数
1029            Output                                :       LPF_Y--当前滤波输出值，执行时间为4us。
1030          -------------------------------------------------------------------------------------------------*/
1031          int16 LPFFunction(int16 Xn1, int16 Xn0, int8 K)
1032          {
1033   1              LPF_K = K;
1034   1              LPF_X = Xn1;
1035   1              LPF_Y = Xn0;
1036   1              SetBit(PI_LPF_CR, LPFSTA);
1037   1              _nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
1038   1              return LPF_Y;
1039   1      }
1040          
1041          /*---------------------------------------------------------------------------*/
1042          /* Name         :       void VSPSample(void)
1043          /* Input        :       NO
1044          /* Output       :       NO
1045          /* Description: VSP采样
1046          /*---------------------------------------------------------------------------*/
1047          //void VSPSample(void)
1048          //{
1049          ////     if((VSP > ONPWMDuty)&&(VSP <= OFFPWMDutyHigh))//在ONPWMDuty-OFFPWMDutyHigh之间，电机有转速�
             -��行
1050          ////            {
1051          ////                    mcSpeedRamp.FlagONOFF = 1;
1052          ////            }
1053          ////     else if((VSP < OFFPWMDuty)||(VSP > OFFPWMDutyHigh))//电机停机
1054          ////            {
1055          ////                    mcSpeedRamp.FlagONOFF = 0;
1056          ////            }
1057          //                      
1058          //              //转速曲线计算
1059          //              if(mcSpeedRamp.FlagONOFF==1)
1060          //              {
1061          //      #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
1062          //      {
1063          //        if(VSP <= MINPWMDuty)    //最小转速运行
1064          //        {
1065          //          mcSpeedRamp.TargetValue = Motor_Min_Speed;
1066          //        }
1067          //        else if(VSP < MAXPWMDuty)//调速
1068          //        {
1069          //          mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(VSP-MINPWMDuty);
1070          //        }
1071          //        else               //最大转速运行
1072          //        {
1073          //          mcSpeedRamp.TargetValue     =       Motor_Max_Speed;
1074          //        }
1075          //      }
1076          ////      #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
1077          ////      {
1078          ////        if(VSP <= MINPWMDuty)    //最小转速运行
1079          ////        {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 19  

1080          ////          mcSpeedRamp.TargetValue = Motor_Min_Power;
1081          ////        }
1082          ////        else if(VSP < MAXPWMDuty)//调速
1083          ////        {
1084          ////          mcSpeedRamp.TargetValue = Motor_Min_Power + POWER_K*(VSP-MINPWMDuty);
1085          ////        }
1086          ////        else                                //最大转速运行
1087          ////        {
1088          ////          mcSpeedRamp.TargetValue   =       Motor_Max_Power;
1089          ////        }
1090          ////      }
1091          //      #endif
1092          //              }
1093          //              else
1094          //              {
1095          //                      mcSpeedRamp.TargetValue =0;
1096          //              }
1097          //}
1098          /*---------------------------------------------------------------------------*/
1099          /* Name         :       void Sleepmode(void)
1100          /* Input        :       NO
1101          /* Output       :       NO
1102          /* Description: 睡眠模式测试
1103          /*---------------------------------------------------------------------------*/
1104           void Sleepmode(void)
1105           {
1106   1                      SleepSet.SleepDelayCout++;
1107   1                      if(SleepSet.SleepDelayCout>=20000)//最大65530，若要再大，需改数据类型
1108   1                      {
1109   2      //                              FOC_EFREQMIN    = -Motor_Omega_Ramp_Min;
1110   2      //                              FOC_EFREQHOLD = -Motor_Omega_Ramp_End;
1111   2                              mcSpeedRamp.TargetValue=0;
1112   2                              MOE     = 0;
1113   2                              ClrBit(DRV_CR, FOCEN);  //关闭FOC
1114   2                              SleepSet.SleepDelayCout=0;
1115   2                              SleepSet.SleepFlag=1;
1116   2                              SetBit(P1_IE, P11);   // config P11 as the source of EXTI1
1117   2                              SetBit(PCON, STOP);
1118   2                      }
1119   1       }
1120          
1121          /*---------------------------------------------------------------------------*/
1122          /* Name         :       void ONOFFControl (void)
1123          /* Input        :       NO
1124          /* Output       :       NO
1125          /* Description:
1126          /*---------------------------------------------------------------------------*/
1127          void ONOFFControl (void)
1128          {
1129   1              static uint8 MotorONFilterTimer=0;
1130   1              static uint8 MotorOFFFilterTimer=0;
1131   1      
1132   1              static uint8  PowerUPRUNProtec = 0;
1133   1              static uint8  PowerUPStatus = 0;
1134   1      
1135   1              static uint8 PWMDutyONOFFCnt1 = 0;
1136   1              static uint8 PWMDutyONOFFCnt2 = 0;
1137   1              
1138   1              static uint8 BrakeDelayTime = 0;
1139   1              
1140   1              static uint8 VSPPrectionCnt = 0;
1141   1              
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 20  

1142   1      //                      if(((mcFocCtrl.mcSpeedVSP > _Q15(ONPWMDuty))&&(mcFocCtrl.mcSpeedVSP <_Q15(ProtecMaxPWMDuty))) && (mcF
             -ocCtrl.MotorONOFFStatus == 0)) //判断正传
1143   1      //                      {
1144   1      //                              
1145   1      //                                      if(PWMDutyONOFFCnt2<5)
1146   1      //                                      {
1147   1      //                                              PWMDutyONOFFCnt2 ++;
1148   1      //                                      }
1149   1      //                                      if(PWMDutyONOFFCnt2 >= 5)
1150   1      //                                      {
1151   1      //                                              PWMDutyONOFFCnt2 =5;
1152   1      //                                              PWMDutyONOFFCnt1 = 0;
1153   1      //                                              OnSelfLock = 0;
1154   1      //                                              mcFocCtrl.MotorONOFFStatus = 1;    //开机标志位置1                        
1155   1      //                                      }
1156   1      //                      }
1157   1      //                      else if((mcFocCtrl.mcSpeedVSP< _Q15(BackONPWMDuty)) && (mcFocCtrl.MotorONOFFStatus == 1)) //判断反
             -转
1158   1      //                      {
1159   1      //                              
1160   1      //                                      if(PWMDutyONOFFCnt2<5)
1161   1      //                                      {
1162   1      //                                              PWMDutyONOFFCnt2 ++;
1163   1      //                                      }
1164   1      //                                      if(PWMDutyONOFFCnt2 >= 5)
1165   1      //                                      {
1166   1      //                                              PWMDutyONOFFCnt2 =5;
1167   1      //                                              PWMDutyONOFFCnt1 = 0;
1168   1      //                                              BackSelfLock = 0;
1169   1      //                                              mcFocCtrl.MotorONOFFStatus = 0;    //开机标志位置1                        
1170   1      //                                      }
1171   1      //                      }
1172   1      //                      
1173   1      //              
1174   1      //              
1175   1      //      if((mcFocCtrl.MotorONOFFStatus)  && (!KeyBrake) && (!OnSelfLock))//正转处理程序
1176   1      //      {
1177   1      
1178   1      //              MotorONFilterTimer++;
1179   1      
1180   1      //              if(MotorONFilterTimer>10)
1181   1      //              {
1182   1      //                              CCCW_Flag = 2;
1183   1      //                              
1184   1      //                      if(ReverseFlag == 1)
1185   1      //                      {
1186   1      //                              mcHall.GivenFRStatus = CW;
1187   1      //                              ClrBit(DRV_CR,DDIR);
1188   1      //                              mcHall.StartAngleOffset   = StartAngleCW;
1189   1      //                              
1190   1      //                              mcState = mcInit;               
1191   1      //                      }
1192   1      //                      
1193   1      //                      else if(ReverseFlag == 0)
1194   1      //                      {
1195   1      //                              KeyBrake = 1;
1196   1      //                      }
1197   1      //                      
1198   1      //                      OnSelfLock = 1;
1199   1      //                      MotorONFilterTimer=0;
1200   1      //              }
1201   1      //      }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 21  

1202   1      
1203   1      //      else if((!mcFocCtrl.MotorONOFFStatus)&&(!KeyBrake) && (!BackSelfLock))//反转处理程序
1204   1      //      {
1205   1      
1206   1      //              MotorONFilterTimer++;
1207   1      
1208   1      //              if(MotorONFilterTimer>10)
1209   1      //              {
1210   1      //                      
1211   1      //                              CCCW_Flag = 3 ;
1212   1      //                      if(ReverseFlag == 1)
1213   1      //                      {
1214   1      
1215   1      //                              mcHall.GivenFRStatus = CCW;
1216   1      //                              SetBit(DRV_CR,DDIR);
1217   1      //                              mcHall.StartAngleOffset   = StartAngleCCW;
1218   1      //                              
1219   1      //                              mcState = mcInit;                               
1220   1      //                      }
1221   1      //                      else if(ReverseFlag ==0)
1222   1      //                      {
1223   1      //                              KeyBrake = 1;
1224   1      //                      }
1225   1      //                      
1226   1      //                      BackSelfLock = 1;
1227   1      //                      MotorONFilterTimer=0;
1228   1      //              }
1229   1      //      }
1230   1      }
1231          
1232          
1233          
1234          void Hall_RTHESTEP_Capture(void)
1235          {
1236   1              static uint8 BaseFlag = 0 ;
1237   1      
1238   1              if(mcHall.HallSpeedInitStatus == 0)
1239   1              {
1240   2                      mcHall.StepTimeCount           = 0;     
1241   2                                                      
1242   2                      mcHall.MC_StepTime[0] = 0;
1243   2                      mcHall.MC_StepTime[1] = 0;
1244   2                      mcHall.MC_StepTime[2] = 0;
1245   2                      mcHall.MC_StepTime[3] = 0;
1246   2                      mcHall.MC_StepTime[4] = 0;
1247   2                      mcHall.MC_StepTime[5] = 0;
1248   2                                                              
1249   2                      mcHall.FlagSpeedCal =0;
1250   2                                      
1251   2                      mcHall.RTheStep      = 0;       
1252   2      
1253   2                      mcHall.HallSpeedInitStatus =1;
1254   2      
1255   2      //              TIM4__CNTR = 0;         
1256   2              }
1257   1              else
1258   1              {
1259   2      //              ClrBit(TIM4_CR1, T4EN);                                            // 0，停止计数；1,使能计数 
1260   2                      
1261   2                      mcHall.MC_StepTime[mcHall.HallStatus_Result-1] = TIM4__CNTR; 
1262   2      
1263   2      //              ClrBit(TIM4_CR1, T4EN);                                            // 0，停止计数；1,使能计数         
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 22  

1264   2                      
1265   2                      TIM4__CNTR = 0;
1266   2                      
1267   2      //              SetBit(TIM4_CR1, T4EN);                                            // 0，停止计数；1,使能计数
1268   2                                                                                      
1269   2                      if(mcHall.HallCommutationCount <= 6)
1270   2                      {
1271   3                              mcHall.HallThetaBase        = TempHallThetaBaseOneStep * mcHall.HallCommutationCount;//TEMPHallThetaBas
             -eOneStep[mcHall.HallCommutationCount-1];
1272   3                              
1273   3                              mcHall.HallSpeedBase        = TempHallSpeedBaseOneStep * mcHall.HallCommutationCount;//TEMPHallSpeedBas
             -eOneStep[mcHall.HallCommutationCount-1];
1274   3                              
1275   3                              BaseFlag =0;
1276   3                              
1277   3                              mcHall.FlagSpeedCal = 1;
1278   3                      } 
1279   2                      else if((mcHall.HallCommutationCount > 6)&&(BaseFlag==0))       
1280   2                      {
1281   3                              mcHall.HallThetaBase = TempHallThetaBase;
1282   3                              
1283   3                              mcHall.HallSpeedBase = TempHallSpeedBase;               
1284   3                              
1285   3                              BaseFlag = 1;                                                   
1286   3                      }
1287   2                      
1288   2                                      
1289   2                      mcHall.PeriodTime = mcHall.MC_StepTime[0] + mcHall.MC_StepTime[1] + mcHall.MC_StepTime[2] + mcHall.MC_St
             -epTime[3] + mcHall.MC_StepTime[4] + mcHall.MC_StepTime[5];    //5.4us
1290   2              
1291   2                      if(mcHall.HallStatus_Result==6)
1292   2                      {                               
1293   3                              mcHall.FlagSpeedCal = 1;
1294   3                      }
1295   2      
1296   2                      MDUAPP1();  //16us
1297   2      //              FOC__RTHESTEP = 0;
1298   2                      if(mcHall.HallFRStatus == 2)
1299   2                      {
1300   3                              FOC__RTHESTEP  = 0- mcHall.RTheStep;            
1301   3                              KeyBrakeAngleCompensate = 0;//4500;
1302   3                      }
1303   2                      else if(mcHall.HallFRStatus == 1)
1304   2                      {
1305   3                              FOC__RTHESTEP  = mcHall.RTheStep;       
1306   3                              KeyBrakeAngleCompensate = 0;                    
1307   3                      }
1308   2                      
1309   2                      if((FOC__UQ >= 0 && HallFRStatusTeat == 2) || (FOC__UQ < 0 && HallFRStatusTeat == 1))
1310   2                      {
1311   3                              SmoothFlag = 1;
1312   3                              SmoothCnt = 0;
1313   3      //                      FOC__RTHESTEP = 0;
1314   3                      }
1315   2                      
1316   2                      if(SmoothFlag == 1 && ((FOC__UQ < 0 && HallFRStatusTeat == 2) || (FOC__UQ >= 0 && HallFRStatusTeat == 1)
             -))
1317   2                      {
1318   3                              SmoothCnt ++;
1319   3                      }
1320   2                      
1321   2                      if(SmoothCnt >= 2)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 23  

1322   2                      {
1323   3                              SmoothFlag = 0;
1324   3                              SmoothCnt = 0;
1325   3                      }
1326   2                      
1327   2                      if(SmoothFlag == 1)
1328   2                              FOC__RTHESTEP = 0;
1329   2                      
1330   2                      if(FOC__RTHESTEP>16385)
1331   2                      {
1332   3                              FOC__RTHESTEP = 10;
1333   3                      }
1334   2                              
1335   2              }
1336   1      }
1337          
1338          void Hall_THETA_Capture(void)  
1339          {
1340   1              static int16 TempHall_Angle = 0;
1341   1              uint8 GiveFR=0;
1342   1              mcHall.HallStatus_Next = HallCommutation(mcHall.GivenFRStatus ,mcHall.HallStatus_Result );  //根据当�
             -�的角度和当前方向获得下一步Hall信号
1343   1      
1344   1              
1345   1              // 当赋值为16400（约90度）时，定子磁场方向与转子磁场相同,电机不会转动，说�
             -��在正常工作时，定子磁场方向与转子磁场方向相差90度
1346   1              mcHall.AngleCompensate = KeyBrakeAngleCompensate;
1347   1              GiveFR = mcHall.HallFRStatus-1;
1348   1              TempHall_Angle = ThetaDispose1 (mcHall.HallStatus_Result,GiveFR,TempAngleOffsetCW,TempAngleOffsetCCW,mcHa
             -ll.AngleCompensate); //10us
1349   1              
1350   1              FOC__THETA   = TempHall_Angle;//算出角度后给FOC强拉
1351   1              
1352   1              if(SmoothFlag == 1)
1353   1              {
1354   2                      FOC__THETA += 2000;
1355   2              }
1356   1      
1357   1              RealAngleGivenToFoc = TempHall_Angle;
1358   1              if(mcHall.HallFRStatus==1)
1359   1          mcHall.NextSectorAngle = TempHall_Angle + 10922;
1360   1        else if(mcHall.HallFRStatus==2)
1361   1          mcHall.NextSectorAngle = TempHall_Angle - 10922;
1362   1              
1363   1              
1364   1      }
1365          
1366          /*-------------------------------------------------------------------------------------------------
1367                  Function Name : int16 RampControl(Ramp_TypeDef *Ramp, int16 Ref)
1368                  Description   : 爬坡目标函数，根据设定爬坡增量把当前值向目标值靠拢
1369                  Input         : Ramp--爬坡对象数组
1370                            Ref-- 爬坡参考值
1371            Output                                :       RampStep--爬坡当前输出
1372          -------------------------------------------------------------------------------------------------*/
1373          int16 RampControl(Ramp_TypeDef *Ramp, int16 Ref)
1374          {
1375   1              Ramp->RampTarget = Ref;
1376   1              Ramp->RampInc = 200;
1377   1              Ramp->RampDec = 200;
1378   1              if(Ramp->RampTarget > Ramp->RampStep)
1379   1              {
1380   2                      if((Ramp->RampStep + Ramp->RampInc) <= Ramp->RampTarget)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 24  

1381   2                      {
1382   3                              Ramp->RampStep += Ramp->RampInc;
1383   3                      }
1384   2                      else
1385   2                      {
1386   3                              Ramp->RampStep = Ramp->RampTarget;
1387   3                      }
1388   2              }
1389   1              else if(Ramp->RampTarget < Ramp->RampStep)
1390   1              {
1391   2                      if((Ramp->RampStep - Ramp->RampDec) >= Ramp->RampTarget)
1392   2                      {
1393   3                              Ramp->RampStep -= Ramp->RampDec;
1394   3                      }
1395   2                      else
1396   2                      {
1397   3                              Ramp->RampStep = Ramp->RampTarget;
1398   3                      }
1399   2              }       
1400   1              return  Ramp->RampStep;
1401   1      }
1402          
1403          /*---------------------------------------------------------------------------*/
1404          /* Name         :       void MotorStop(void)
1405          /* Input        :       NO
1406          /* Output       :       NO
1407          /* Description:
1408          /*---------------------------------------------------------------------------*/
1409          void MotorStop(void)
1410          {
1411   1              if(mcFocCtrl.mcStopMode == FreeStop)
1412   1              {
1413   2                      mcFocCtrl.CurrentStopMode = FreeStop;
1414   2              }
1415   1              else if((mcFocCtrl.mcStopMode == QuickBrakeStop)||(mcFocCtrl.mcStopMode == SlowBrakeStop))   
1416   1              {
1417   2                      mcFocCtrl.CurrentStopMode = mcFocCtrl.mcStopMode;
1418   2              }
1419   1              else if(mcFocCtrl.mcStopMode == SlowStop)   
1420   1              {
1421   2                      mcFocCtrl.CurrentStopMode = SlowStop;
1422   2              }
1423   1              else if(mcFocCtrl.mcStopMode == OppoAngleBrakeStop)
1424   1              {
1425   2                      mcFocCtrl.CurrentStopMode = OppoAngleBrakeStop;
1426   2              }
1427   1      
1428   1      
1429   1      
1430   1              if(mcFocCtrl.CurrentStopMode == FreeStop)
1431   1              {
1432   2                              FOC_CR1 = 0x00;
1433   2                              MOE = 0;
1434   2                              /*关闭FOC*/
1435   2                              ClrBit(DRV_CR, FOCEN);                                          
1436   2              }
1437   1              else if((mcFocCtrl.CurrentStopMode == QuickBrakeStop)||(mcFocCtrl.CurrentStopMode == SlowBrakeStop))
1438   1              {
1439   2                      if(mcFocCtrl.CurrentStopMode == QuickBrakeStop)
1440   2                      {
1441   3                              DRV_DR = 1.0*DRV_ARR;                                 // brake PWM DUTY                 
1442   3          }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 25  

1443   2                      else if(mcFocCtrl.CurrentStopMode == SlowBrakeStop)
1444   2                      {                       
1445   3                              DRV_DR = mcFocCtrl.BrakeDuty*DRV_ARR;                               // brake PWM DUTY   
1446   3          }   
1447   2                      
1448   2                      mcFocCtrl.mcIqref =0;
1449   2      //              SpeedRamp.SPEEDRampCnt = 0;
1450   2      //              SpeedRamp.RampStep =    mcFocCtrl.mcIqref;
1451   2                      
1452   2      //              FOC__UQ = SpeedRamp.RampStep;
1453   2                      //FOC_QMAX              = SpeedRamp.RampStep;
1454   2                      //FOC_QMIN              = SpeedRamp.RampStep;
1455   2                                              
1456   2                      DRV_CMR |= 0x15;
1457   2                      
1458   2                      ClrBit(DRV_CR,OCS);        //OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
1459   2                                     
1460   2                      SetBit(DRV_CR, FOCEN);                  
1461   2                      MOE     = 1;
1462   2                      
1463   2              }
1464   1              else if (mcFocCtrl.CurrentStopMode == SlowStop) 
1465   1              {
1466   2                      if((SpeedRamp.RampStep<=MinPWMDuty)||(mcFocCtrl.MotorOFFTimes==0)||(mcFocCtrl.McSlowStopCnt >= SlowStopT
             -ime))
1467   2                      {
1468   3                              mcFocCtrl.McRunFlag = 0;
1469   3                              
1470   3      //                      SpeedRamp.RampStep = 0;
1471   3                              
1472   3                              mcFocCtrl.FreeStopDecStatus        = 0;
1473   3                              
1474   3                              FOC_CR1 = 0x00;
1475   3                              MOE = 0;
1476   3                              /*关闭FOC*/
1477   3                              ClrBit(DRV_CR, FOCEN);                                                  
1478   3                      }
1479   2                      else if(SpeedRamp.RampStep > MinPWMDuty)
1480   2                      {
1481   3                              if(mcFocCtrl.McRunFlag)
1482   3                              {                                                                                                                                        
1483   4                                      mcFocCtrl.FreeStopDecStatus        = 1;
1484   4                              }
1485   3          }
1486   2      
1487   2        }
1488   1              else if (mcFocCtrl.CurrentStopMode == OppoAngleBrakeStop)
1489   1              {
1490   2                      if(mcHall.HallSpeed <= 7000)
1491   2                      {
1492   3                              FOC_CR1 = 0x00;
1493   3                              MOE = 0;
1494   3                              /*关闭FOC*/
1495   3                              ClrBit(DRV_CR, FOCEN);                                          
1496   3                      }
1497   2                      else if(mcHall.HallSpeed > 7000)
1498   2                      {
1499   3                              
1500   3                              FOC_CR1 = 0x00;
1501   3                              MOE = 0;
1502   3                              /*关闭FOC*/
1503   3                              ClrBit(DRV_CR, FOCEN);                                          
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 26  

1504   3      
1505   3      //                      FOC_QMAX                        = QOUTMAX;
1506   3      //                      FOC_QMIN                        = QOUTMIN;
1507   3      
1508   3      //                      
1509   3      //                      FOC_IQREF = -700;
1510   3      //                      
1511   3      //                                              DRV_DR = 0.9*DRV_ARR;                                 // brake PWM DUTY                 
1512   3      //              
1513   3      ////            mcFocCtrl.mcIqref =0;
1514   3      ////            SpeedRamp.SPEEDRampCnt = 0;
1515   3      ////            SpeedRamp.RampStep =    mcFocCtrl.mcIqref;
1516   3      //              
1517   3      //              //FOC__UQ = SpeedRamp.RampStep;
1518   3                      //FOC_QMAX              = SpeedRamp.RampStep;
1519   3                      //FOC_QMIN              = SpeedRamp.RampStep;
1520   3      //                                      
1521   3      ////            DRV_CMR |= 0x15;
1522   3      //              
1523   3      //              ClrBit(DRV_CR,OCS);        //OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
1524   3      //                             
1525   3      //              ClrBit(DRV_CR, FOCEN);                  
1526   3      //              MOE     = 1;
1527   3      
1528   3      //                      KeyBrakeRTheStep = 0;
1529   3      //                      KeyBrakeAngleCompensate = 17000;
1530   3      //                      
1531   3      //                      //SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle);                                                 
1532   3      //                      
1533   3      //                      mcFocCtrl.OpenDutyCycle = 1000;
1534   3      //                      SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle);
1535   3      //                      FOC__UQ = SpeedRamp.RampStep;
1536   3      
1537   3      ////                    mcHall.GivenFRStatus = 1;
1538   3      ////                    mcHall.StartAngleOffset   = StartAngleCCW;
1539   3      ////                    SetBit(DRV_CR,DDIR);
1540   3      //                      OppoBrakeFlag = 1;
1541   3                      }
1542   2                      
1543   2              }
1544   1      }
1545          
1546          void  mcTailWindDispose (void)
1547          {
1548   1              int16 TempCalCulFOC_QM;
1549   1              
1550   1                      if((mcState== mcRun)||(mcState== mcStart))
1551   1                      {
1552   2                              if(mcHall.HallDirectStartStatus)
1553   2                              {
1554   3                                      if((Time.mcRunStateCount<60000)&&(Time.mcRunStateCount!=1))
1555   3                                      {
1556   4                                              Time.mcRunStateCount++;
1557   4                                      }
1558   3                                      
1559   3                                      if(Time.mcRunStateCount>36)
1560   3                                      {
1561   4                                              MOE = 1;
1562   4                                                                      
1563   4                                              SpeedFollow(983,QOUTMAX,QOUTMIN);  // SRampInc 爬坡增量 327 - 1638
1564   4                                                                                                              
1565   4                                              if(Time.mcRunStateCount>80)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 27  

1566   4                                              {
1567   5                                                      mcHall.HallDirectStartStatus =0;                                                                                                                                        
1568   5                                              }
1569   4                                      }
1570   3                                      else if(Time.mcRunStateCount==1)
1571   3                                      {                                                                                                                               
1572   4                                                      mcFocCtrl.SpeedV_k = mcHall.LastHallSpeed/32767.0;
1573   4                                                                                      
1574   4                                                      if(mcHall.LastHallSpeed < 2000)
1575   4                                                      { 
1576   5                                                              TempCalCulFOC_QM =      _Q15(0.1)*mcFocCtrl.SpeedV_k +_Q15(0.10);         
1577   5                                                      }
1578   4                                                      else if(mcHall.LastHallSpeed <= 4000)
1579   4                                                      {
1580   5                                                              TempCalCulFOC_QM =      _Q15(0.1)*mcFocCtrl.SpeedV_k +_Q15(0.11);       
1581   5                                                      } 
1582   4                                                      else if(mcHall.LastHallSpeed <= 6000)
1583   4                                                      {
1584   5                                                              TempCalCulFOC_QM =      _Q15(0.1)*mcFocCtrl.SpeedV_k +_Q15(0.25);       
1585   5                                                      } 
1586   4                                                      else if(mcHall.LastHallSpeed <= 8000)
1587   4                                                      {
1588   5                                                              TempCalCulFOC_QM =      _Q15(0.20)*mcFocCtrl.SpeedV_k +_Q15(0.35);       //0.35
1589   5                                                      } 
1590   4                                                      else if(mcHall.LastHallSpeed <= 9000)
1591   4                                                      {
1592   5                                                              TempCalCulFOC_QM =      _Q15(0.25)*mcFocCtrl.SpeedV_k +_Q15(0.40);       //0.35
1593   5                                                      } 
1594   4                                                      else if(mcHall.LastHallSpeed <= 14000)
1595   4                                                      {
1596   5                                                              TempCalCulFOC_QM =      _Q15(0.40)*mcFocCtrl.SpeedV_k +_Q15(0.45);      
1597   5                                                      } 
1598   4                                                      else 
1599   4                                                      {
1600   5                                                              if(mcHall.LastHallSpeed>=15928)
1601   5                                                              {
1602   6                                                                      mcHall.LastHallSpeed =15928;
1603   6                    }
1604   5                                                              
1605   5      //                                                      if(mcHall.GivenFRStatus == CW)
1606   5      //                                                      {
1607   5                                                                      TempCalCulFOC_QM =      _Q15(0.50)*mcFocCtrl.SpeedV_k +_Q15(0.55);
1608   5      //              }
1609   5      //                                                      else
1610   5      //                                                      {
1611   5      //                                                              TempCalCulFOC_QM =      _Q15(0.50)*mcFocCtrl.SpeedV_k +_Q15(0.55);      
1612   5      //                                                      }
1613   5                                                      }
1614   4                                                                                                      
1615   4                                                      FOCQMAXLimit(TempCalCulFOC_QM);             //输出占空比限制                     
1616   4                                              
1617   4      //                                              SpeedRamp.RampStep = TempCalCulFOC_QM;
1618   4      
1619   4                                                      SpeedRamp.RampTarget = TempCalCulFOC_QM;                                        
1620   4                                              
1621   4                                                      mcHall.HallCommutationCount=1;
1622   4                                              
1623   4                                                      Time.mcRunStateCount =2;                                
1624   4                                      }
1625   3                              }
1626   2                              else
1627   2                              {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 28  

1628   3                                      Time.mcRunStateCount=0;
1629   3                              }                       
1630   2                      }
1631   1      }
1632          
1633          
1634          //均值限流处理
1635          void RMSCurrentLimit(void)
1636          {
1637   1              if(mcFocCtrl.mcADCCurrentbus >= mcFocCtrl.RMSLimitTargetCurrent+50)
1638   1              {
1639   2                              if(mcFocCtrl.mcIqref > 150)
1640   2                              {
1641   3                                      mcFocCtrl.mcIqref -= 150;
1642   3      //                              SpeedRamp.RampStep = mcFocCtrl.mcIqref;
1643   3                                      
1644   3      //                              FOC__UQ = SpeedRamp.RampStep;
1645   3      //                              FOC_QMAX                = SpeedRamp.RampStep;
1646   3      //                              FOC_QMIN                = SpeedRamp.RampStep;
1647   3                              }
1648   2              }
1649   1              else if((mcFocCtrl.mcADCCurrentbus > mcFocCtrl.RMSLimitTargetCurrent)&&((mcFocCtrl.mcADCCurrentbus < mcFo
             -cCtrl.RMSLimitTargetCurrent+20)))
1650   1              {
1651   2                              if(mcFocCtrl.mcIqref > 10)
1652   2                              {
1653   3                                      mcFocCtrl.mcIqref -= 10;
1654   3      //                              SpeedRamp.RampStep = mcFocCtrl.mcIqref;
1655   3      
1656   3      //                              FOC__UQ = SpeedRamp.RampStep;
1657   3      //                              FOC_QMAX                = SpeedRamp.RampStep;
1658   3      //                              FOC_QMIN                = SpeedRamp.RampStep;
1659   3                              }
1660   2              }
1661   1      }
1662          
1663          
1664          
1665          /*---------------------------------------------------------------------------*/
1666          /* Name         :       void HallLockDisposeFuntion (void)
1667          /* Input        :       NO
1668          /* Output       :       NO
1669          /* Description:
1670          /*---------------------------------------------------------------------------*/
1671          void HallLockCheckFuntion (void)
1672          {
1673   1              if(mcHall.HALLLockStatus)
1674   1              {
1675   2                      mcHall.mcStallTimeCnt++;
1676   2                      
1677   2                      if(mcHall.mcStallTimeCnt >= StallTime)
1678   2                      {
1679   3                              mcHall.mcStallTimeCnt = StallTime;
1680   3          }
1681   2        }
1682   1              else
1683   1              {
1684   2                      mcHall.mcStallTimeCnt =0;
1685   2        }
1686   1                      
1687   1              if(HALLC)
1688   1              {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 29  

1689   2                      mcHall.HALLHCCWCCWTime ++;
1690   2                      
1691   2                      #if (StallProtectEnable)
                              {
                                      if((mcState == mcRun)||(mcState == mcStart))
                                      {                               
                                              mcHall.HALLLockHCTime++;
                                              if(mcHall.HALLLockHCTime>=StallDetectTime)
                                              {
                                                      mcHall.HALLLockHCTime = StallDetectTime;
                                                      mcHall.HALLLockStatus = 1;                                      
                                              }
                                      }
                              }
                              #endif
1704   2              }
1705   1              else
1706   1              {
1707   2                      mcHall.HALLLockHCTime = 0;
1708   2                      mcHall.HALLHCCWCCWTime =0;
1709   2              }
1710   1              
1711   1              
1712   1              if(HALLB)
1713   1              {
1714   2                      mcHall.HALLHBCWCCWTime ++;
1715   2                      
1716   2                      #if (StallProtectEnable)
                              {
                                      if((mcState == mcRun)||(mcState == mcStart))
                                      {                               
                                              mcHall.HALLLockHBTime++;
                                              if(mcHall.HALLLockHBTime>=StallDetectTime)
                                              {
                                                      mcHall.HALLLockHBTime = StallDetectTime;
                                                      mcHall.HALLLockStatus =1;
                                              }
                                      }
                              }
                              #endif
1729   2              }
1730   1              else
1731   1              {
1732   2                      mcHall.HALLLockHBTime = 0;
1733   2                      mcHall.HALLHBCWCCWTime =0;
1734   2              }
1735   1              
1736   1              if(HALLA)
1737   1              {
1738   2                      mcHall.HALLHACWCCWTime++;
1739   2                      
1740   2                      #if (StallProtectEnable)
                              {
                                      if((mcState == mcRun)||(mcState == mcStart))
                                      {                               
                                              mcHall.HALLLockHATime++;
                                              if(mcHall.HALLLockHATime >= StallDetectTime)
                                              {
                                                      mcHall.HALLLockHATime = StallDetectTime;
                                                      mcHall.HALLLockStatus = 1;
                                              }
                                      }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/25/2018 17:39:38 PAGE 30  

                              }
                              #endif
1753   2              }
1754   1              else
1755   1              {
1756   2                      mcHall.HALLLockHATime = 0;
1757   2                      mcHall.HALLHACWCCWTime =0;
1758   2              }
1759   1      }


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   4419    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =    213      21
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
   IDATA SIZE       =    134    ----
   BIT SIZE         =   ----    ----
   EDATA SIZE       =   ----    ----
   HDATA SIZE       =   ----    ----
   XDATA CONST SIZE =   ----    ----
   FAR CONST SIZE   =   ----    ----
END OF MODULE INFORMATION.


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