C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE ADDFUNCTION
OBJECT MODULE PLACED IN .\Output\AddFunction.obj
COMPILER INVOKED BY: C:\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             : Vina Peng,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;
*** ERROR C129 IN LINE 18 OF ..\User\Source\Application\AddFunction.c: missing ';' before 'mcFaultSource'
  19          PWMINPUTCAL        idata   mcPwmInput;
  20          FaultVarible       idata   mcFaultDect;
  21          MotorRSDTypeDef    idata   RSDDetect;
  22          CurrentVarible     idata   mcCurVarible;
  23          ProtectVarible     idata   mcProtectTime;
  24          
  25          FOCCTRL            xdata   mcFocCtrl;
  26          ADCSample          xdata   AdcSampleValue;
  27          ONVarible          xdata   ONOFFTest;
  28          MCLedDisplay       xdata   mcLedDisplay;
  29          MCRAMP             xdata   mcSpeedRamp;
  30          SLEEPMODE          xdata   SleepSet;
  31          MotorFRTypeDef     xdata   mcFRState;
  32          int16              xdata   VSP;
  33          
  34          const int16  PowerGiven[10] ={0,500,1000,2200,2900,4300,5900,7300,7900,8550};
  35          /*---------------------------------------------------------------------------*/
  36          /* Name         :       int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  37          /* Input        :       INVlaue，OutLastValue
  38          /* Output       :       int16的变量
  39          /* Description: 滤波函数,用乘法器做的
  40          /*---------------------------------------------------------------------------*/
  41          int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  42          {
  43                  int16 Result = 0;
  44                  MDU_MA = (INVlaue-OutLastValue);
  45                  MDU_MB = (int16)480;                                            /*写被乘数和乘数*/
  46          
  47                  Result = MDU_MB;
  48                  Result += OutLastValue;
  49                  return(Result);
  50          }
  51          
  52          /*---------------------------------------------------------------------------*/
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 2   

  53          /* Name         :       void FaultProcess(void)
  54          /* Input        :       NO
  55          /* Output       :       NO
  56          /* Description: 保护处理函数，关闭FOC输出，同时将状态变为mcFault
  57          /*---------------------------------------------------------------------------*/
  58          void FaultProcess(void)
  59          {
  60                  MOE     = 0;
  61                  ClrBit(DRV_CR, FOCEN);  //关闭FOC
  62                  mcState = mcFault;
  63          
  64          }
  65          
  66          /*---------------------------------------------------------------------------*/
  67          /* Name         :       int16 Abs_F16(int16 value)
  68          /* Input        :       value
  69          /* Output       :       int16
  70          /* Description: 对变量取16位的绝对值
  71          /*---------------------------------------------------------------------------*/
  72          uint16 Abs_F16(int16 value)
  73          {
  74                  if(value < 0)
  75                  {
  76                          return (- value);
  77                  }
  78                  else
  79                  {
  80                          return (value);
  81                  }
  82          }
  83          /*---------------------------------------------------------------------------*/
  84          /* Name         :       int32 Abs_F32(int32 value)
  85          /* Input        :       value
  86          /* Output       :       int16
  87          /* Description: 对变量取16位的绝对值
  88          /*---------------------------------------------------------------------------*/
  89          uint32 Abs_F32(int32 value)
  90          {
  91                  if(value < 0)
  92                  {
  93                          return (- value);
  94                  }
  95                  else
  96                  {
  97                          return (value);
  98                  }
  99          }
 100          /*---------------------------------------------------------------------------*/
 101          /* Name         :       void APP_DIV(void)
 102          /* Input        :       void
 103          /* Output       :       void
 104          /* Description: 将所有用到除法的地方，放在同一个中断，以避免中断串扰
 105          /*---------------------------------------------------------------------------*/
 106          void APP_DIV(void)
 107          {
 108              if( mcPwmInput.PWMDivFlag==1)  //启动除法器，避免与过调值中的除法冲突
 109              {
 110                 mcPwmInput.PWMDuty = MDU_DIV_IDATA_U32(&mcPwmInput.pwm.PWMCompareAMP, &mcPwmInput.PWMARRUpdate);
 111                 mcPwmInput.PWMDivFlag=0;
 112              }
 113              if( mcFocCtrl.ESDIVFlag==1)  //启动除法器，避免与过调值中的除法冲突
 114              {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 3   

 115                 mcFocCtrl.SQUSpeedDIVEs = MDU_DIV_XDATA_U32(&mcFocCtrl.SQUSysSpeed,&mcFocCtrl.EsValue);
 116                 mcFocCtrl.ESDIVFlag=0;
 117              }
 118          }
 119          /*---------------------------------------------------------------------------*/
 120          /* Name         :       void PWMInputCapture(void)
 121          /* Input        :       NO
 122          /* Output       :       NO
 123          /* Description: 输入PWM处理
 124          /*---------------------------------------------------------------------------*/
 125          void PWMInputCapture(void)
 126          {
 127            uint16 MotorControlVSP;
 128          
 129                   if(mcPwmInput.PWMUpdateFlag==1)  // 有新的duty更新
 130                   {
 131                if((Abs_F32(mcPwmInput.PWMCompare-mcPwmInput.PWMCompareOld)<50)// 两次比较值相近，减少读�
             -��率
 132                  &&(Abs_F32(mcPwmInput.PWMARROld-mcPwmInput.PWMARR)<50)// 两次周期值相近，减少读错率
 133          //        &&((100<mcPwmInput.PWMARR)&&(mcPwmInput.PWMARR<6000))// 周期值在一定范围内才认为有�
             -��，即一定频率范围
 134                  &&(mcPwmInput.PWMDivFlag==0))
 135                  {
 136                    mcPwmInput.PWMFlag=1;                               // PWMFlag置1期间，不读取TIM3__DR和T
             -IM3__ARR;，防止出错
 137                    mcPwmInput.pwm.PWMCompareUpdate[0]=(mcPwmInput.PWMCompare>>1);// 对其乘以32768
 138                    mcPwmInput.PWMARRUpdate=mcPwmInput.PWMARR;
 139                    mcPwmInput.PWMDivFlag=1;                            // 启动除法
 140                    mcPwmInput.PWMFlag=0;
 141                  }
 142                 if(mcPwmInput.PWMDivFlag==0)
 143                  {
 144                    if(mcPwmInput.PWMcnt<3)//2次求平均值
 145                    {
 146                      mcPwmInput.PWMcnt++;
 147                      mcPwmInput.PWMVSum +=mcPwmInput.PWMDuty;
 148                    }
 149                    else
 150                    {
 151                      MotorControlVSP= (mcPwmInput.PWMVSum >>1);//注意其有一个右移与前面的比较值标�
             -�化有关
 152                      mcPwmInput.PWMVSum=0;
 153                      mcPwmInput.PWMcnt =0;
 154                    }
 155                     MotorControlVSP=mcPwmInput.PWMDuty;
 156                   if((MotorControlVSP > ONPWMDuty)&&(MotorControlVSP <=(OFFPWMDutyHigh+1)))
 157                    {
 158                      mcSpeedRamp.FlagONOFF = 1;
 159                    }
 160                   else if(MotorControlVSP < OFFPWMDuty)//||(MotorControlVSP >= OFFPWMDutyHigh))
 161                    {
 162                      mcSpeedRamp.FlagONOFF = 0;
 163                    }
 164          
 165                    //转速曲线计算
 166                    if(mcSpeedRamp.FlagONOFF==1)
 167                    {
 168                      if(MotorControlVSP <= MINPWMDuty)
 169                      {
 170                        mcSpeedRamp.TargetValue = Motor_Min_Speed;
 171                      }
 172                      else if(MotorControlVSP < MAXPWMDuty)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 4   

 173                      {
 174                        mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(MotorControlVSP-MINPWMDuty);
 175                      }
 176                      else
 177                      {
 178                        mcSpeedRamp.TargetValue   =       Motor_Max_Speed;
 179                      }
 180                    }
 181                    else
 182                    {
 183                      mcSpeedRamp.TargetValue =0;
 184                    }
 185                  }
 186          
 187                mcPwmInput.PWMUpdateFlag =0;
 188                mcPwmInput.PWMCompareOld=mcPwmInput.PWMCompare;//将此次比较值赋值给上次比较值
 189                mcPwmInput.PWMARROld=mcPwmInput.PWMARR;//将此次周期值赋值给上次周期值
 190                    }
 191          }
 192          /*****************************************************************************
 193           * Function:             void   Fault_OverVoltage(mcFaultVarible *h_Fault)
 194           * Description:  过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值�
             -�，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
 195                           同理，欠压保护。
 196                                 电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复
             -值以下时，计数器加一，超过200次后，恢复。根据档位信息来决定恢复到哪个状态。
 197           * Parameter:            mcFaultVarible *h_Fault
 198           * Return:                       no
 199           *****************************************************************************/
 200          void Fault_OverUnderVoltage(FaultVarible *h_Fault)
 201          {
 202                  //过压保护
 203                          if(mcFaultSource == FaultNoSource)//程序无其他保护下
 204                          {
 205                                          if(mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE)    //母线电压大于过压保护值时，计数，超�
             -�20次，判断为过压保护，关闭输出;反之，计数器慢慢减
 206                                          {
 207                                                  h_Fault->OverVoltDetecCnt++;
 208                                                  if(h_Fault->OverVoltDetecCnt > 20)//检测100ms
 209                                                  {
 210                                                          h_Fault->OverVoltDetecCnt = 0;
 211                                                          mcFaultSource=FaultOverVoltage;
 212                                                          FaultProcess();
 213                                                  }
 214                                          }
 215                                          else
 216                                          {
 217                                                  if(h_Fault->OverVoltDetecCnt>0)
 218                                                  {
 219                                                          h_Fault->OverVoltDetecCnt--;
 220                                                  }
 221                                          }
 222          
 223                                  //欠压保护
 224          
 225                                          if(mcFocCtrl.mcDcbusFlt< UNDER_PROTECT_VALUE)
 226                                          {
 227                                                  h_Fault->UnderVoltDetecCnt++;
 228          
 229                                                  if(h_Fault->UnderVoltDetecCnt > 20)//检测100ms
 230                                                  {
 231                                                          h_Fault->UnderVoltDetecCnt = 0;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 5   

 232                                                          mcFaultSource=FaultUnderVoltage;
 233                                                          FaultProcess();
 234                                                  }
 235                                          }
 236                                          else
 237                                          {
 238                                                  if(h_Fault->UnderVoltDetecCnt>0)
 239                                                  {
 240                                                          h_Fault->UnderVoltDetecCnt--;
 241                                                  }
 242                                          }
 243                          }
 244          
 245                          /*******过压欠压保护恢复*********/
 246                          if((mcState == mcFault) &&((mcFaultSource==FaultUnderVoltage)||(mcFaultSource==FaultOverVoltage)))
 247                          {
 248                                  if((mcFocCtrl.mcDcbusFlt< OVER_RECOVER_VALUE)&&(mcFocCtrl.mcDcbusFlt> UNDER_RECOVER_VALUE))
 249                                  {
 250                                          h_Fault->VoltRecoverCnt++;
 251                                          if(h_Fault->VoltRecoverCnt>200)//连续检测1s，若正常则恢复
 252                                          {
 253                    mcState = mcReady;
 254                                                  mcFaultSource=FaultNoSource;
 255                                                  h_Fault->VoltRecoverCnt = 0;
 256                                          }
 257                                  }
 258                                  else
 259                                  {
 260                                          h_Fault->VoltRecoverCnt = 0;
 261                                  }
 262                   }
 263          }
 264          
 265          /*****************************************************************************
 266           * Function:             void Fault_Overcurrent(CurrentVarible *h_Cur)
 267           * Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则O
             -verCurCnt加1。
 268                                                                           连续累加3次，判断为软件过流保护。执行时间约30.4us。
 269           * Parameter:            mcFaultVarible *h_Fault
 270           * Return:                       no
 271           *****************************************************************************/
 272          void Fault_Overcurrent(CurrentVarible *h_Cur)
 273          {
 274                  if((mcState == mcRun)||(mcState == mcStart))                                            // check over current in rum and open mode
 275                  {
 276          
 277                          h_Cur->Abs_ia = Abs_F16(FOC__IA);
 278                          h_Cur->Abs_ib = Abs_F16(FOC__IB);
 279                          h_Cur->Abs_ic = Abs_F16(FOC__IC);
 280                          if(h_Cur->Abs_ia> h_Cur->Max_ia)                      // 此部分既用于软件过流保护，又用�
             -�缺相保护
 281                          {
 282                                   h_Cur->Max_ia = h_Cur->Abs_ia;
 283                          }
 284                          if(h_Cur->Abs_ib > h_Cur->Max_ib)
 285                          {
 286                                   h_Cur->Max_ib = h_Cur->Abs_ib;
 287                          }
 288                          if(h_Cur->Abs_ic > h_Cur->Max_ic)
 289                          {
 290                                   h_Cur->Max_ic = h_Cur->Abs_ic;
 291                          }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 6   

 292          
 293                          if((h_Cur->Max_ia>=OverSoftCurrentValue)||(h_Cur->Max_ib>=OverSoftCurrentValue)||(h_Cur->Max_ic>=OverSof
             -tCurrentValue))
 294                          {
 295                                  h_Cur->OverCurCnt++;
 296                                  if(h_Cur->OverCurCnt>=3)
 297                                  {
 298                  GP36=~GP36;
 299                                          h_Cur->Max_ia=0;
 300                                          h_Cur->Max_ib=0;
 301                                          h_Cur->Max_ic=0;
 302                                          h_Cur->OverCurCnt=0;
 303                                    mcFaultSource=FaultSoftOVCurrent;
 304                                    FaultProcess();
 305                                  }
 306                          }
 307              else
 308                          {
 309                                  if(h_Cur->OverCurCnt>0)
 310                                  {
 311                                          h_Cur->OverCurCnt--;
 312                                  }
 313                          }
 314                  }
 315          }
 316          
 317          /*****************************************************************************
 318           * Function:             void   Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
 319           * Description:  软硬件过流保护恢复
 320           * Parameter:            mcFaultVarible *h_Fault
 321           * Return:                       no
 322           *****************************************************************************/
 323          void Fault_OverCurrentRecover(FaultVarible *h_Fault)
 324          {
 325                  if((mcState == mcFault)&&((mcFaultSource==FaultSoftOVCurrent)||(mcFaultSource==FaultHardOVCurrent))&&(mcP
             -rotectTime.CurrentPretectTimes<5))
 326                  {
 327                          h_Fault->CurrentRecoverCnt++;
 328                          if(h_Fault->CurrentRecoverCnt>=OverCurrentRecoverTime)//200*5=1s
 329                          {
 330                                  h_Fault->CurrentRecoverCnt=0;
 331                                  mcProtectTime.CurrentPretectTimes++;
 332                                  mcState = mcReady;
 333                                  mcFaultSource=FaultNoSource;
 334              }
 335            }
 336          }
 337          
 338          /*****************************************************************************
 339           * Function:             void   Fault_Start(mcFaultVarible *h_Fault)
 340           * Description:  启动保护函数，电机运行状态下，电机在前5s估算转速达到堵转保护
             -值或者5s后反电动势值太低(此方法未验证)
 341                            或4s内还在CtrlMode状态，即速度低于MOTOR_LOOP_RPM，程序判断为启动失�
             -��，电机停机。
 342                            当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进
             -入校准状态，等待重启。
 343           * Parameter:            mcFaultVarible *h_Fault
 344           * Return:                       no
 345           *****************************************************************************/
 346            void Fault_Start(FaultVarible *h_Fault)
 347            {
 348                          /*******启动保护恢复*********/
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 7   

 349          //              h_Fault->mcEsValue = FOC__ESQU;
 350          
 351                          if(mcState == mcRun)
 352                          {
 353                                  //方法一，5s内速度大于最大速度，同时反电动势值低于一定值
 354                                  if(h_Fault->StartSpeedCnt<=1000)
 355                                  {
 356                                    h_Fault->StartSpeedCnt++;
 357                                          if((mcFocCtrl.SpeedFlt > Motor_Max_Speed)&&(mcFocCtrl.EsValue<50))
 358                                          {
 359                                                  h_Fault->StartSpeedCnt = 0;
 360                                                  mcFaultSource=FaultStart;
 361                                                  FaultProcess();
 362                                                  mcProtectTime.SecondStartTimes++;
 363                                                  mcProtectTime.StartFlag  =  1;
 364                  }
 365                }
 366               //方法二
 367                                  if(h_Fault->StartEsCnt<=1200)//前6s，等待1.5s后，开始判断ES，如果超过一定次数，则�
             -��败
 368                                  {
 369                                          h_Fault->StartEsCnt++;
 370                                          h_Fault->StartDelay++;
 371                                          if(h_Fault->StartDelay>=300)                            // 1.5s
 372                                          {
 373                                                   h_Fault->StartDelay=300;
 374                                                   if((mcFocCtrl.EsValue <5))//&&(mcFocCtrl.CtrlMode==0))
 375                                                          {
 376                                                                  h_Fault->StartESCount++;
 377                                                                  if(h_Fault->StartESCount>=20)
 378                                                                  {
 379                                                                          mcFaultSource=FaultStart;
 380                                                                          FaultProcess();
 381                                                                          mcProtectTime.SecondStartTimes++;
 382                                                                          h_Fault->StartDelay=0;
 383                                                                          h_Fault->StartESCount=0;
 384                                                                          mcProtectTime.StartFlag  =  2;
 385                                                                  }
 386                                                          }
 387                      else
 388                                                          {
 389                                                                  if(h_Fault->StartESCount>0)
 390                                                                          h_Fault->StartESCount--;
 391                                                          }
 392                                   }
 393                           }
 394               else
 395               {
 396                 h_Fault->StartESCount=0;
 397               }
 398                                  //方法三，长时间在CtrlMode=0状态
 399                                  if(mcFocCtrl.CtrlMode==0)         //
 400                                  {
 401                                          h_Fault->StartFocmode++;
 402                                          if(h_Fault->StartFocmode>=400)
 403                                          {
 404                                                  h_Fault->StartFocmode=0;
 405                                                  mcFaultSource=FaultStart;
 406                                                  FaultProcess();
 407                                                  mcProtectTime.SecondStartTimes++;
 408                    mcProtectTime.StartFlag  =  3;
 409                  }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 8   

 410                                  }
 411                    }
 412              #if (!StartONOFF_Enable)
 413              {
 414                           if((mcFaultSource==FaultStart)&&(mcState == mcFault)&&(mcProtectTime.SecondStartTimes<=StartProtectRest
             -artTimes))
 415                           {
 416                                   mcFaultSource=FaultNoSource;
 417                                   mcState = mcInit;
 418                           }
 419              }
 420             #endif
 421          
 422                  }
 423           /*****************************************************************************
 424           * Function:             void   Fault_Stall(mcFaultVarible *h_Fault)
 425           * Description:  堵转保护函数，有三种保护方式，
 426                                 第一种，
 427                                 第二种，电机运行状态下，延迟4s判断，估算速度绝对值超过堵转速�
             -��连续5次；
 428                                 第三种，电机运行状态下，当U,V两相电流绝对值大于堵转电流保护值
             -连续6次；
 429                                 当以上三种的任何一种保护触发时，电机停机，程序判断为堵转保护
             -；
 430                                 当堵转保护状态下，U相采集值低于堵转恢复值时，若堵转次数小于�
             -�等于堵转重启次数8次，
 431                                 程序延迟mcStallRecover重新启动，进行校准状态。
 432           * Parameter:            mcFaultVarible *h_Fault
 433           * Return:                       no
 434           *****************************************************************************/
 435          //堵转保护
 436          void Fault_Stall(FaultVarible *h_Fault)
 437          {
 438          //      h_Fault->mcEsValue = FOC__ESQU;
 439                  static uint16 UserStallRecoverTime = 0;
 440                  if(Time.mcRunStateCount<10000)
 441                  {
 442                          UserStallRecoverTime = 10;
 443                  }
 444                  else
 445                  {
 446                          UserStallRecoverTime = StallRecoverTime;
 447                  }
 448                  if((mcState == mcRun)&&(Time.mcRunStateCount>4000))
 449                  {
 450          //              if(h_Fault->StallDelayCnt <=1000)//5s
 451          //              {
 452          //                      h_Fault->StallDelayCnt ++;
 453          //              }
 454          //              else
 455          //              {
 456                      //method 1，判断反电动势太小或当反电动势太小，转速太大
 457                                  if((mcFocCtrl.EsValue< 40)||((FOC__EOME > _Q15(100.0/MOTOR_SPEED_BASE))&&(mcFocCtrl.EsValue< 100)))
 458                                  {
 459                                          h_Fault->StallDectEs++;
 460                                          if(h_Fault->StallDectEs >= 10)
 461                                          {
 462                                                  h_Fault->StallDectEs=0;
 463                                                  mcFaultSource=FaultStall;
 464                                                  mcProtectTime.StallTimes++;
 465                              mcProtectTime.StallFlag  =  1;
 466                                                  FaultProcess();
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 9   

 467                                                  
 468                                          }
 469                                  }
 470                                  else
 471                                  {
 472                          if(     h_Fault->StallDectEs>0)
 473                                            h_Fault->StallDectEs--;
 474                                  }
 475                                  //method 2，判断速度低于堵转最小值或者超过堵转最大值
 476                                  if((mcFocCtrl.SpeedFlt<Motor_Stall_Min_Speed)||(mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
 477                                  {
 478                                          h_Fault->StallDectSpeed++;
 479                                          if(h_Fault->StallDectSpeed>=8)
 480                                          {
 481                                                  h_Fault->StallDectSpeed=0;
 482                                                  mcFaultSource=FaultStall;
 483                                                  mcProtectTime.StallTimes++;
 484                              mcProtectTime.StallFlag =2;
 485                                                  FaultProcess();
 486                                          }
 487                                  }
 488                                  else
 489                                  {
 490                          if(h_Fault->StallDectSpeed>0)
 491                              h_Fault->StallDectSpeed--;
 492                                  }
 493          //    }
 494            }
 495              #if (!StartONOFF_Enable)
 496              {
 497                           /*******堵转保护恢复*********/
 498                  if((mcFaultSource==FaultStall)&&(mcState == mcFault)&&(mcProtectTime.StallTimes<=4))
 499                  {
 500                    h_Fault->StallReCount++;
 501                    if(h_Fault->StallReCount>=UserStallRecoverTime)
 502                    {
 503                      h_Fault->StallReCount=16000;
 504                      mcFaultSource=FaultNoSource;
 505                      mcState = mcInit;
 506                    }
 507                  }
 508                  else
 509                  {
 510                    h_Fault->StallReCount=0;
 511                  }
 512              }
 513             #endif
 514          
 515          }
 516           /*****************************************************************************
 517           * Function:             void   Fault_phaseloss(mcFaultVarible *h_Fault)
 518           * Description:  缺相保护函数，当电机运行状态下，10ms取三相电流的最大值，
 519                                 1.5s判断各相电流最大值，若存在两相电流值大于一定值，而第三相�
             -�流值却非常小，则判断为缺相保护，电机停机；
 520           * Parameter:            mcFaultVarible *h_Fault
 521           * Return:                       no
 522           *****************************************************************************/
 523            void Fault_phaseloss(FaultVarible *h_Fault)
 524            {
 525              if(mcState == mcRun)
 526              {
 527                  h_Fault->Lphasecnt++;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 10  

 528                  if(h_Fault->Lphasecnt>100)//100*5=500ms
 529                  {
 530                      h_Fault->Lphasecnt=0;
 531                      if((mcCurVarible.Max_ia<PhaseLossLowCurrent)&&(mcCurVarible.Max_ib>PhaseLossHighCurrent)&&(mcC
             -urVarible.Max_ic>PhaseLossHighCurrent))
 532                      {
 533                          h_Fault->AOpencnt++;
 534                      }
 535                      else
 536                      {
 537                          h_Fault->AOpencnt =0;
 538                      }
 539                      if((mcCurVarible.Max_ib<PhaseLossLowCurrent)&&(mcCurVarible.Max_ia>PhaseLossHighCurrent)&&(mcC
             -urVarible.Max_ic>PhaseLossHighCurrent))
 540                      {
 541                          h_Fault->BOpencnt++;
 542                      }
 543                      else
 544                      {
 545                          if(h_Fault->BOpencnt>0)
 546                              h_Fault->BOpencnt --;
 547                      }
 548                      if((mcCurVarible.Max_ic<PhaseLossLowCurrent)&&(mcCurVarible.Max_ib>PhaseLossHighCurrent)&&(mcC
             -urVarible.Max_ia>PhaseLossHighCurrent))
 549                      {
 550                          h_Fault->COpencnt++;
 551                      }
 552                      else
 553                      {
 554                          if(h_Fault->COpencnt>0)
 555                              h_Fault->COpencnt --;
 556                      }
 557                      mcCurVarible.Max_ia = 0;
 558                      mcCurVarible.Max_ib = 0;
 559                      mcCurVarible.Max_ic = 0;
 560                      if(h_Fault->AOpencnt > 1|| h_Fault->BOpencnt > 1 || h_Fault->COpencnt > 1)
 561                      {
 562                          mcProtectTime.LossPHTimes++;
 563                          mcFaultSource=FaultLossPhase;
 564                          FaultProcess();
 565                      }
 566                  }
 567              }
 568          
 569              #if (!StartONOFF_Enable)
 570              {
 571                  /*******缺相保护恢复*********/
 572                  if((mcFaultSource==FaultLossPhase)&&(mcState == mcFault)&&
 573                  (mcProtectTime.LossPHTimes<5))//可重启5次
 574                  {
 575                      h_Fault->mcLossPHRecCount++;
 576                      if(h_Fault->mcLossPHRecCount>=PhaseLossRecoverTime)
 577                      {
 578                      h_Fault->AOpencnt=0;
 579                      h_Fault->BOpencnt=0;
 580                      h_Fault->COpencnt=0;
 581                      mcState = mcReady;
 582                      mcFaultSource=FaultNoSource;
 583                      }
 584                  }
 585                  else
 586                  {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 11  

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

 647          }
 648          
 649          /*---------------------------------------------------------------------------*/
 650          /* Name         :       void ONOFF_Starttest(void)
 651          /* Input        :       NO
 652          /* Output       :       NO
 653          /* Description: 启动测试
 654          /*---------------------------------------------------------------------------*/
 655          void ONOFF_Starttest(ONVarible  *h_test)
 656          {
 657                  if(h_test->ONOFF_Flag==1)
 658                  {
 659                           h_test->ON_Count++;
 660                          if(h_test->ON_Count>StartON_Time)
 661                          {
 662                                  h_test->ON_Count=0;
 663                                  h_test->ONOFF_Times++;
 664                                  h_test->ONOFF_Flag=0;
 665                            mcSpeedRamp.FlagONOFF = 0;
 666                                  mcSpeedRamp.TargetValue = 0;
 667          
 668                          }
 669            }
 670                  else
 671                  {
 672                          if(mcState!=mcFault)
 673                          {
 674                            h_test->OFF_Count++;
 675                                  if(h_test->OFF_Count>StartOFF_Time)
 676                                  {
 677                                          h_test->OFF_Count=0;
 678                                          h_test->ONOFF_Flag=1;
 679                                           mcSpeedRamp.FlagONOFF = 1;
 680                                           mcSpeedRamp.TargetValue = Motor_Min_Speed;
 681          
 682                                  }
 683                    }
 684          
 685            }
 686          }
 687          
 688          /*---------------------------------------------------------------------------*/
 689          /* Name         :       void Speed_response(void)
 690          /* Input        :       NO
 691          /* Output       :       NO
 692          /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制
             -、恒功率控制
 693          /*---------------------------------------------------------------------------*/
 694          void Speed_response(void)
 695          {
 696          
 697                  //遥控更新转速
 698                  if(IRControl.FlagSpeed )
 699                  {
 700                          mcSpeedRamp.TargetValue = IRControl.SpeedLevel[IRControl.TargetSpeed];
 701                          IRControl.FlagSpeed=0;
 702                  }       
 703                  if((mcState ==mcRun)||(mcState ==mcStop))
 704                  {
 705                  if(mcFocCtrl.OUTLoopCtrl.PIRunDelayTime < mcFocCtrl.OUTLoopCtrl.Close_loop_delay_time)
 706                  {
 707                      mcFocCtrl.OUTLoopCtrl.PIRunDelayTime++;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 13  

 708          //            if(mcFocCtrl.SpeedFlt >Motor_Loop_Speed)//>=MOTOR_LOOP_RPM
 709          //            {
 710          //                                      mcFocCtrl.CtrlMode= 1;
 711          //                                      FOC_DQKP = DQKP;
 712          //                                      FOC_DQKI = DQKI;
 713                                                  #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
 714                                                  {
 715                                                          mcSpeedRamp.ActualValue = FOC__EOME;
 716                                                  }
 717                                                  #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                                                      {
                                                              mcSpeedRamp.ActualValue = mcFocCtrl.Powerlpf;
                                                      }
                                                      #endif
 722          
 723                                                  mcFocCtrl.OUTLoopCtrl.TorqueLoopTime= 0;
 724                                                  PI_UK = mcFocCtrl.mcIqref;
 725          //                              }
 726                  }
 727                  else
 728                  {
 729                      if(mcFocCtrl.CtrlMode==0)
 730                      {
 731                          FOC_DQKP = DQKP;
 732                                          FOC_DQKI = DQKI;
 733                          mcFocCtrl.CtrlMode= 1;
 734                      }
 735                     mcFocCtrl.OUTLoopCtrl.TorqueLoopTime++;
 736                  }
 737                  if(mcFocCtrl.OUTLoopCtrl.TorqueLoopTime>SPEED_LOOP_TIME)
 738                  {
 739          
 740                      mcFocCtrl.OUTLoopCtrl.TorqueLoopTime=0;
 741                      #if (OUTLoop_Mode== OUTLoop_Disable)
                          {
                              mcFocCtrl.mcIqref = FOC_IQREF;
                              if (FOC_IQREF < QOUTVALUE)
                              {
                                  mcFocCtrl.mcIqref += QOUTINC;
                                  if (mcFocCtrl.mcIqref > QOUTVALUE) mcFocCtrl.mcIqref = QOUTVALUE;
                                  FOC_IQREF = mcFocCtrl.mcIqref;
                              }
                              else if (FOC_IQREF > QOUTVALUE)
                              {
                                  mcFocCtrl.mcIqref -= QOUTINC;
                                  if (mcFocCtrl.mcIqref < QOUTVALUE) mcFocCtrl.mcIqref = QOUTVALUE;
                                  FOC_IQREF = mcFocCtrl.mcIqref;
                              }
                          }
                          #elif (OUTLoop_Mode== OUTLoop_Enable)
 758                      {
 759                          mc_ramp(&mcSpeedRamp);
 760                          if(mcSpeedRamp.FlagONOFF)
 761                          {
 762                              #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
 763                              {
 764                                  FOC_IQREF= HW_One_PI(mcSpeedRamp.ActualValue- mcFocCtrl.SpeedFlt);//COMPESATE是�
             -�为补偿
 765                              }
 766                              #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                                  {
                                      FOC_IQREF= HW_One_PI(mcSpeedRamp.ActualValue- mcFocCtrl.Powerlpf);
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 14  

                                  }
                                  #endif
 771                          }
 772                          else
 773                          {
 774                              FOC_IQREF=HW_One_PI(-mcFocCtrl.SpeedFlt);
 775                          }
 776                      }
 777                      #endif //END OUTLoop_Mode
 778                      if(FOC_EK2 < OBS_K2T_Actual-10)
 779                      {
 780                          mcFocCtrl.Smo_EK2=FOC_EK2;
 781                          mcFocCtrl.Smo_EK2+=10;
 782                          FOC_EK2=mcFocCtrl.Smo_EK2;
 783                      }
 784                      else if(FOC_EK2 > OBS_K2T_Actual+10)
 785                      {
 786                          mcFocCtrl.Smo_EK2=FOC_EK2;
 787                          mcFocCtrl.Smo_EK2-=10;
 788                          FOC_EK2=mcFocCtrl.Smo_EK2;
 789                      }
 790                      else
 791                      {
 792                          FOC_EK2=OBS_K2T_Actual;
 793                      }
 794                  }
 795              }
 796                  #if (FRDetectMethod!=FOCMethod)
                      {
                              if(mcState==mcRun)
                              {
                                      if(mcFocCtrl.RunStateCnt<15000)//15s的延迟，用于清RSDCCWFlag,BEMFCCWFlag
                                      {
                                              mcFocCtrl.RunStateCnt++;
                                      }
                                      else
                                      {
                                              BEMFDetect.BEMFCCWFlag= 0;
                                              RSDDetect.RSDCCWFlag  = 0;
                                      }
                              }
                      }
                      #endif
 812          
 813          
 814          }
 815          
 816          /*---------------------------------------------------------------------------*/
 817          /* Name         :       void FGOutput(void)
 818          /* Input        :       NO
 819          /* Output       :       NO
 820          /* Description: FG信号输出
 821          /*---------------------------------------------------------------------------*/
 822          void FGOutput(void)
 823          {//FG可用中断FGIF进行判断
 824                  if(mcState==mcRun)
 825                  {
 826                          if((FOC__THETA>=0)&&(FOC__THETA<32768))//0-180
 827                          {
 828                                  ResetFGPin;
 829                          }
 830                          else if((FOC__THETA>=32768)&&(FOC__THETA<65536))//180-360
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 15  

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

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

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

1015                  Output            :     PI_UK--当前PI输出值,执行时间us
1016          -------------------------------------------------------------------------------------------------*/
1017          int16 HW_One_PI(int16 Xn1)
1018          {
1019                    PI_EK =  Xn1;       //填入EK
1020                    PI_LPF_CR |= 0x02;      // Start PI
1021                    _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1022                    PI_UK+=(SKP/4096 +1);
1023                    return PI_UK;
1024          }
1025          /*-------------------------------------------------------------------------------------------------
1026                  Function Name : int16 LPF(int16 Xn1, int16 Xn0, int8 K)
1027                  Description   : LFP控制
1028                  Input         : Xn1--当前输入值
1029                                  Xn0--上一次滤波输出值
1030                                                                                  K--LPF滤波系数
1031            Output                                :       LPF_Y--当前滤波输出值，执行时间为4us。
1032          -------------------------------------------------------------------------------------------------*/
1033          int16 LPFFunction(int16 Xn1, int16 Xn0, int8 K)
1034          {
1035                  LPF_K = K;
1036                  LPF_X = Xn1;
1037                  LPF_Y = Xn0;
1038                  SetBit(PI_LPF_CR, LPFSTA);
1039                  _nop_();_nop_();_nop_();_nop_();_nop_();
1040                  return LPF_Y;
1041          }
1042          
1043          /*---------------------------------------------------------------------------*/
1044          /* Name         :       void VSPSample(void)
1045          /* Input        :       NO
1046          /* Output       :       NO
1047          /* Description: VSP采样
1048          /*---------------------------------------------------------------------------*/
1049          void VSPSample(void)
1050          {
1051                  /*****VREF的采样获取值并滤波******/
1052                  AdcSampleValue.ADCVref = (ADC6_DR<<3);//采集对应端口
1053                  VSP=LPFFunction(AdcSampleValue.ADCVref,VSP,30);//低通滤波
1054          
1055                   if((VSP > ONPWMDuty)&&(VSP <= OFFPWMDutyHigh))//在ONPWMDuty-OFFPWMDutyHigh之间，电机有转速运�
             -�
1056                          {
1057                                  mcSpeedRamp.FlagONOFF = 1;
1058                          }
1059                   else if((VSP < OFFPWMDuty)||(VSP > OFFPWMDutyHigh))//电机停机
1060                          {
1061                                  mcSpeedRamp.FlagONOFF = 0;
1062                          }
1063                          //转速曲线计算
1064                          if(mcSpeedRamp.FlagONOFF==1)//
1065                          {
1066                #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
1067                {
1068                  if(VSP <= MINPWMDuty)    //最小转速运行
1069                  {
1070                    mcSpeedRamp.TargetValue = Motor_Min_Speed;
1071                  }
1072                  else if(VSP < MAXPWMDuty)//调速
1073                  {
1074                    mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(VSP-MINPWMDuty);
1075                  }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 19  

1076                  else                 //最大转速运行
1077                  {
1078                    mcSpeedRamp.TargetValue       =       Motor_Max_Speed;
1079                  }
1080                }
1081                #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                    {
                      if(VSP <= MINPWMDuty)    //最小转速运行
                      {
                        mcSpeedRamp.TargetValue = Motor_Min_Power;
                      }
                      else if(VSP < MAXPWMDuty)//调速
                      {
                        mcSpeedRamp.TargetValue = Motor_Min_Power + POWER_K*(VSP-MINPWMDuty);
                      }
                      else                 //最大转速运行
                      {
                        mcSpeedRamp.TargetValue       =       Motor_Max_Power;
                      }
                    }
                    #endif
1097          
1098                          }
1099                          else
1100                          {
1101                                  mcSpeedRamp.TargetValue =0;
1102                          }
1103          }
1104          /*---------------------------------------------------------------------------*/
1105          /* Name         :       void Sleepmode(void)
1106          /* Input        :       NO
1107          /* Output       :       NO
1108          /* Description: 睡眠模式测试
1109          /*---------------------------------------------------------------------------*/
1110           void Sleepmode(void)
1111           {
1112                                  SleepSet.SleepDelayCout++;
1113                          if(SleepSet.SleepDelayCout>=20000)//最大65530，若要再大，需改数据类型
1114                          {
1115          //                              FOC_EFREQMIN    = -Motor_Omega_Ramp_Min;
1116          //                              FOC_EFREQHOLD = -Motor_Omega_Ramp_End;
1117                                  mcSpeedRamp.TargetValue=0;
1118                                  MOE     = 0;
1119                                  ClrBit(DRV_CR, FOCEN);  //关闭FOC
1120                                  SleepSet.SleepDelayCout=0;
1121                                  SleepSet.SleepFlag=1;
1122                                  SetBit(P1_IE, P11);   // config P11 as the source of EXTI1
1123                                  SetBit(PCON, STOP);
1124                          }
1125          
1126           }
1127          /*---------------------------------------------------------------------------*/
1128          /* Name         :       void StarRampDealwith(void)
1129          /* Input        :       NO
1130          /* Output       :       NO
1131          /* Description:
1132          /*---------------------------------------------------------------------------*/
1133          void StarRampDealwith(void)
1134          {
1135                          if((mcState == mcRun))
1136                          {
1137                                  if((mcFocCtrl.State_Count>=(4000))&&(mcSpeedRamp.FlagONOFF))
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 20  

1138                                  {
1139                                          if( mcFocCtrl.mcIqref < IQ_Start_CURRENT)
1140                                          {
1141                                                  mcFocCtrl.mcIqref       += 20;
1142                                                  FOC_IQREF = mcFocCtrl.mcIqref;  
1143                                                  PI_UK   =       mcFocCtrl.mcIqref;
1144                                          }
1145                                  }
1146                                  //启动后电流逐渐下降为IQ_RUN_CURRENT
1147                                  if((mcFocCtrl.State_Count<(4000))&&(mcFocCtrl.State_Count>(3000)))
1148                                  {
1149                                          if(mcFocCtrl.mcIqref > (IQ_RUN_CURRENT))
1150                                          {
1151                                                  mcFocCtrl.mcIqref -= 10;
1152                                                  FOC_IQREF = mcFocCtrl.mcIqref;  
1153                                                  PI_UK   =       mcFocCtrl.mcIqref;
1154                                          }                       
1155                                  }
1156                                  if(mcFocCtrl.State_Count == 5700)//2300
1157                                  {
1158                                          FOC_EKP = OBSW_KP_GAIN_RUN;                               // 估算器里的PI的KP
1159                                          FOC_EKI = OBSW_KI_GAIN_RUN;                                                 // 估算器里的PI的KI
1160                                  }
1161                                  else if(mcFocCtrl.State_Count == 5400)//2000
1162                                  {
1163                                          FOC_EKP = OBSW_KP_GAIN_RUN1;                            // 估算器里的PI的KP
1164                                          FOC_EKI = OBSW_KI_GAIN_RUN1;                                              // 估算器里的PI的KI
1165                                  }
1166                                  else if(mcFocCtrl.State_Count == 5000)//1600
1167                                  {
1168                                          FOC_EKP = OBSW_KP_GAIN_RUN2;                            // 估算器里的PI的KP
1169                                          FOC_EKI = OBSW_KI_GAIN_RUN2;                                              // 估算器里的PI的KI
1170                                  }
1171                                  else if(mcFocCtrl.State_Count ==4000)//1200
1172                                  {
1173                                          FOC_EKP = OBSW_KP_GAIN_RUN3;                            // 估算器里的PI的KP
1174                                          FOC_EKI = OBSW_KI_GAIN_RUN3;                                              // 估算器里的PI的KI
1175                                  }
1176                                  else if(mcFocCtrl.State_Count == 3000)
1177                                  {
1178                                          FOC_EKP = OBSW_KP_GAIN_RUN4;                            // 估算器里的PI的KP
1179                                          FOC_EKI = OBSW_KI_GAIN_RUN4;                                              // 估算器里的PI的KI
1180                                  }
1181                                  else if(mcFocCtrl.State_Count == 2000)
1182                                  {
1183                                          FOC_EKP = OBSW_KP_GAIN_RUN5;                            // 估算器里的PI的KP
1184                                          FOC_EKI = OBSW_KI_GAIN_RUN5;                                              // 估算器里的PI的KI
1185                                  }
1186                                  else if(mcFocCtrl.State_Count == 10)
1187                                  {
1188                                          FOC_EKP = OBSW_KP_GAIN_RUN6;                            // 估算器里的PI的KP
1189                                          FOC_EKI = OBSW_KI_GAIN_RUN6;                                              // 估算器里的PI的KI
1190                                  }
1191                                  else;
1192                          }
1193          }
1194          
1195          
1196          //void IRONOFF_Control(void)
1197          //{     
1198          //      if((IRControl.ONOFFStatus)&&(mcState == mcBrake))
1199          //      {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 21  

1200          //              mcState = mcInit;
1201          //              IRControl.FlagONOFF = 0;
1202          //      }
1203          //      else if((IRControl.ONOFFStatus)&&(mcState!= mcBrake))
1204          //      {
1205          //              IRControl.FlagONOFF = 0;
1206          //      }               
1207          //      
1208          //      if((!IRControl.ONOFFStatus)&&(mcFocCtrl.mcSysSpeed<_Q15(MOTOR_POWER_OFF_RPM/MOTOR_SPEED_BASE)))
1209          //      {                       
1210          //              //关机前先把电流将至较小值再关闭输出
1211          //              if( mcFocCtrl.mcIqref > POWER_OFF_CURRENT)
1212          //              {
1213          //                      mcFocCtrl.mcIqref-= 5;
1214          //                      FOC_IQREF = mcFocCtrl.mcIqref;  
1215          //                      PI_UK     =     mcFocCtrl.mcIqref;
1216          //              }
1217          //              else
1218          //              {               
1219          //                      mcState                                 = mcBreak;      
1220          //                      mcFocCtrl.mcFaultSource = FaultNoSource;                
1221          //                      
1222          //                      //关机后堵转保护次数和缺相保护次数清零
1223          //                      mcFaultDect.mcStallRestartTimes = 0;
1224          //                      mcFaultDect.mcLossPHTimes               = 0;
1225          //                      mcFaultDect.OverCurrentTimes    = 0;
1226          //                      IRControl.FlagONOFF                     = 0;
1227          //              }
1228          //      }
1229          //      else if((!IRControl.ONOFFStatus)&&(mcState == mcBreak))
1230          //      {
1231          //              IRControl.FlagONOFF = 0;
1232          //      }
1233          //}
1234          
1235          
1236          void CurrentZeroDetect(void)
1237          {
1238                  int16 TempCurrentA;
1239                  static int16 TempADC0Value=0; 
1240                  if(mcState==mcTailWind)
1241                  {
1242                          static uint16 MaxCurrent =0;
1243                          static uint16 CurrentDetectTime =0;
1244                          static uint16 CurrentDetectRound =0;
1245                          
1246          //              if(mcFRState.FR == CW)
1247          //              {
1248                          TempADC0Value = LPFFunction((ADC0_DR<<3), TempADC0Value, 12);
1249          //              }
1250          //              else if(mcFRState.FR    == CCW)
1251          //              {
1252          //                      TempADC0Value = LPFFunction((ADC1_DR<<3), TempADC0Value, 12);
1253          //              }
1254                          if(TailWindDetect.TailWindBrakeStatus)
1255                          {
1256                  //              FOC_CR1    = 0x06;                                      // enable break function
1257                  //              FOC_ARR    = PWM_VALUE_LOAD;
1258                  //              FOC_SWDUTY = FOC_ARR; 
1259                                  MOE = 0;
1260                                  FOC_CR1 = 0x00;
1261                                  ClrBit(DRV_CR, FOCEN);
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/26/2018 08:28:40 PAGE 22  

1262                                  DRV_DR  = DRV_ARR+1;
1263                                  DRV_CMR &= 0xFFC0;
1264                                  DRV_CMR |= 0x015;            // 三相下桥臂通，刹车
1265                                  ClrBit(DRV_CR, OCS);         // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
1266                                  MOE = 1;
1267                                  
1268                                  mcFocCtrl.State_Count = TailWindDetect.BrakeRelaxTime;
1269                                  
1270                                  CurrentDetectTime ++ ;
1271                                  if(CurrentDetectTime < 400)
1272                                  {
1273                                          TempCurrentA = abs((TempADC0Value+100) - mcCurOffset.IuOffset); 
1274                                          if(TempCurrentA > MaxCurrent)
1275                                          {
1276                                                  MaxCurrent = TempCurrentA;
1277                                          }
1278                                  }
1279                                  else
1280                                  {
1281                                          CurrentDetectTime =0;
1282                                          if(MaxCurrent < BrakeCurrentTh)
1283                                          {
1284                                                  TailWindDetect.TailWindBrakeStatus = 0;
1285                                                  TailWindDetect.BrakeCurrentDetectStatus = 1;
1286                                                  mcFocCtrl.State_Count = TailWindDetect.BrakeRelaxTime;
1287                                          }
1288                                          MaxCurrent = 0;
1289                                          CurrentDetectRound ++;
1290                                          if(CurrentDetectRound > 20)
1291                                          {
1292                                                  CurrentDetectRound = 0;
1293                                                  TailWindDetect.TailWindBrakeStatus = 0;
1294                                                  TailWindDetect.BrakeCurrentDetectStatus = 1;
1295                                                  mcFocCtrl.State_Count = TailWindDetect.BrakeRelaxTime;
1296                                          }
1297                                  }
1298                          }
1299                          else
1300                          {
1301                                  CurrentDetectTime = 0;
1302                                  CurrentDetectRound = 0;
1303                                  MaxCurrent = 0;
1304                          }
1305                  }
1306          }
1307          

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