GearedMotors700W/HVBLDC_Sensored.c

/* ==============================================================================
System Name:  	HVBLDC_Sensored

File Name:	  	HVBLDC_Sensored.C

Description:	Primary system file for the (Trapezoidal) Sensored BLDC Control 
				Using Hall Effect Sensors  
===================================================================================*/

// Include header files used in the main function

#include "PeripheralHeaderIncludes.h"
#define   MATH_TYPE      IQ_MATH
#include "IQmathLib.h"
#include "HVBLDC_Sensored.h"
#include "HVBLDC_Sensored-Settings.h"
#include <math.h>
#include "UserCan.h"
#include "lowpass.h"
#include "var.h"
#ifdef FLASH
#pragma CODE_SECTION(MainISR,"ramfuncs");
void MemCopy();
void InitFlash();
#endif
#define TIMER1_PER1 60
#define TIMER1_PER2 500000
#define TIMER1_PER 30000000
// Prototype statements for functions found within this file.
interrupt void MainISR(void);//主中断
interrupt void xint1_isr(void);//FO中断
interrupt void xint2_isr(void);//HALL A中断
interrupt void EPWM1TZint_isr(void);//TZ中断  过流中断
void DeviceInit();
void HVDMC_Protection(void);
void speed_direction(Uint16 hall,Uint16 hall_last);//使用中断计算转速
void speed_cal(void);//使用中断计算转速
void pwmlimit_speed(void);//
void ClosePwm(void);
void OpenPwm(void);

// State Machine function prototypes
//------------------------------------
int16 MastIsrTimePercent(void);
void taskfree(void);
void ServiceDog(void);
void EnableDog(void);

// Used for running BackGround in flash, and ISR in RAM
//速度计算相关

int16 Xint2Cnt = 0;

extern Uint16 *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;

int16	SerialCommsTimer;
// Global variables used in this system
HALL3 hall1 = HALL3_DEFAULTS;
SPEED_MEAS_CAP speed1 = SPEED_MEAS_CAP_DEFAULTS;
PWMGEN pwm1 = PWMGEN_DEFAULTS;

_iq20 kscaler = _IQ20(90*3.3/4096/2.3);
float32 T = 0.001/ISR_FREQUENCY;    // Samping period (sec), see parameter.h 
Uint16 BackTicker = 0;
Uint16 PreviousState;
Uint16 ClosedFlag = 0;
Uint32 VirtualTimer = 0;
Uint16 ILoopFlag = FALSE;
Uint16 SpeedLoopFlag = FALSE;
int16  DFuncDesired = 0x0300;      // Desired duty cycle (Q15)
int16  DfuncTesting = 500;//0x0300;//占空比
int16 Delay30 = 0x3FFF;
int16 DelayFlag = 0;
Uint16 AlignFlag = 0x000F; 
Uint16 LoopCount = 0;
Uint16 TripFlagDMC=0;				//PWM trip status 

#if (BUILDLEVEL<= LEVEL2)
Uint32 CmtnPeriodTarget = 0x00000500;
Uint32 CmtnPeriodSetpt = 0x00002000;//要大于CmtnPeriodTarget才能使用--
Uint32 RampDelay = 10;
#else
Uint32 CmtnPeriodTarget = 0x00000450;
Uint32 CmtnPeriodSetpt = 0x00001500;
Uint32 RampDelay = 10;
#endif


_iq DCbus_current=0;

_iq Speedgd=0;
_iq tempIdc=0;

int32 Rt;
Uint16 ch1=0;
Uint16 ch2=0;
Uint16 ch3=0;

// Used for ADC Configuration 
int 	ChSel[16] =   {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int		TrigSel[16] = {5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5};
int     ACQPS[16] =   {8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8};




// Instance PI regulator to regulate the DC-bus current and speed



// Instance a PWM driver instance


// Instance a PWM DAC driver instance
PWMDAC pwmdac1 = PWMDAC_DEFAULTS;

// Instance a Hall effect driver


// Instance a ramp controller to smoothly ramp the frequency


// Instance a RAMP2 Module
RMP2 rmp2 = RMP2_DEFAULTS;

// Instance a RAMP3 Module
RMP3 rmp3 = RMP3_DEFAULTS;

// Instance a MOD6 Module
MOD6CNT mod1 = MOD6CNT_DEFAULTS;

// Instance a IMPULSE Module
IMPULSE impl1 = IMPULSE_DEFAULTS;

// Instance a SPEED_PR Module


// Create an instance of DATALOG Module  950:3A 但是只有外界扭矩低于36N才能启动起来
Uint16 pwm_limit_table1[5] = {950,1300,1650,1930,2320};//0 50 100 150 200
////                     0,    100,  200, 300, 400 500 600  700   800  900 1000
Uint16 pwm_limit_table2[]= {950,1650,2320,3050,3750,4460,5200,5929,6623,7358,8050,
                           8744,9400,10165,10848,11600,12290,12953,13684,14310,15050,
                           15760,16434,17054,17750,18493,19134,19830,20507,21260,21926,
                           22602,23232,23935,24609,25240};

int32 pwmlimitrate = 32768;
void main(void)
{
	
	DeviceInit();	// Device Life support & GPIO		
	InitCan();

// Only used if running from FLASH
// Note that the variable FLASH is defined by the compiler

#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The  RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files. 
	MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)



// Initialize all the Device Peripherals:
// This function is found in DSP280x_CpuTimers.c
   InitCpuTimers();
   
// Configure CPU-Timer 0 to interrupt every ISR Period:
// 60MHz CPU Freq, ISR Period (in uSeconds)
// This function is found in DSP280x_CpuTimers.c
   ConfigCpuTimer(&CpuTimer0, 60, 1000/ISR_FREQUENCY);//40K
   StartCpuTimer0();

// Configure CPU-Timer 1,2 for background loops 
   ConfigCpuTimer(&CpuTimer1, TIMER1_PER1, TIMER1_PER2);//2hz
  // ConfigCpuTimer(&CpuTimer2, 60, 50000);//20HZ
   StartCpuTimer1();
  // StartCpuTimer2();

	
// Reassign ISRs. 
// Reassign the PIE vector for TINT0 to point to a different 
// ISR then the shell routine found in DSP280x_DefaultIsr.c.
// This is done if the user does not want to use the shell ISR routine
// but instead wants to use their own ISR.


	EALLOW;	// This is needed to write to EALLOW protected registers
	PieVectTable.TINT0 = &MainISR;//40KHZ
	PieVectTable.XINT1 = &xint1_isr;
	PieVectTable.XINT2 = &xint2_isr;
	PieVectTable.EPWM1_TZINT = &EPWM1TZint_isr;
	EDIS;   // This is needed to disable write to EALLOW protected registers

// Enable PIE group 1 interrupt 7 for TINT0
    PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
    PieCtrlRegs.PIEIER1.bit.INTx4 = 1;          // Enable PIE Gropu 1 INT4   //XINT1
    PieCtrlRegs.PIEIER1.bit.INTx5 = 1;          // Enable PIE Gropu 1 INT5   //XINT2
// Enable CPU INT1 for TINT0:
	IER |= M_INT1;
	PieCtrlRegs.PIEIER2.bit.INTx1 = 1;
	IER |= M_INT2;

// Enable Global realtime interrupt DBGM
    EALLOW;
    GpioIntRegs.GPIOXINT1SEL.bit.GPIOSEL = 16;  //定位到哪个引脚中断
    GpioIntRegs.GPIOXINT2SEL.bit.GPIOSEL = 24;   // XINT2 is GPIO24
    EDIS;
    XIntruptRegs.XINT1CR.bit.POLARITY = 1;                 // 1为上升沿，0和2为下降沿，3为双边沿
    XIntruptRegs.XINT2CR.bit.POLARITY = 3;                 // 1为上升沿，0和2为下降沿，3为双边沿
    //中断配置步骤-----1,开启模块中断使能，
    XIntruptRegs.XINT1CR.bit.ENABLE = 1;
    XIntruptRegs.XINT2CR.bit.ENABLE = 1;        // Enable XINT2
// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM
	
	
// Initialize PWM module
    pwm1.PeriodMax = (SYSTEM_FREQUENCY/PWM_FREQUENCY)*1000/2;  // Asymmetric PWM  20kHZ  3000
    pwm1.DutyFunc  = 0;  //  占空比ALIGN_DUTY/32767       // DutyFunc = Q15
	BLDCPWM_INIT_MACRO(1,2,3,&pwm1);

// Initialize PWMDAC module
	pwmdac1.PeriodMax  = 500;		 			// @60Mhz, 1500->20kHz, 1000-> 30kHz, 500->60kHz
	pwmdac1.HalfPerMax = pwmdac1.PeriodMax/2;	// Needed to adjust the duty cycle range in the macro
	PWMDAC_INIT_MACRO(6,pwmdac1) ;	// PWM 6A,6B
	PWMDAC_INIT_MACRO(7,pwmdac1) ;	// PWM 7A,7B

// Initialize Hall module   
    hall1.DebounceAmount = 0;
    hall1.Revolutions    = 1;
	HALL3_INIT_MACRO(&hall1);//读hall值 初始化HallGpioBuffer,HallGpioAccepted

// Initialize the SPEED_PR module
   speed1.InputSelect = 0;
   speed1.BaseRpm = (Uint32)120*BASE_FREQ/POLES;//(Uint32)120*BASE_FREQ/POLES/6;//// 使用一个换相周期//额定转速 = 60*(基频/(极数/2)) = 60*f/极对数
   speed1.SpeedScaler = (Uint32)((Uint32)ISR_FREQUENCY*10000/BASE_FREQ);//保留一位小数//40000/f

	
// For the kits < Rev 1.1 -------------------------------------------------	 
	 ChSel[0]=15;	// Dummy meas. avoid 1st sample issue Rev0 Picollo
	 ChSel[1]=2;	// ChSelect: ADC B7-> Phase A Voltage
	 ChSel[2]=3;	// ChSelect: ADC B6-> Phase B Voltage
	 ChSel[3]=4;	// ChSelect: ADC B4-> Phase C Voltage
	 ChSel[4]=7;	// ChSelect: ADC A2-> DC Bus  vol

     ChSel[5]=6;   // DC current 放大20倍 Imeasure = I*5mΩ*20
     ChSel[6]=5;   // NTC
     ChSel[7]=1;   // VOT

//-------------------------------------------------------------------------	 
	 
	 ADC_MACRO_INIT(ChSel,TrigSel,ACQPS);
	 


// Initialize RMP2 module
	rmp2.Out = (int32)0;
	rmp2.Ramp2Delay = 0x00000001;
    rmp2.Ramp2Max = 32767/2;
    rmp2.Ramp2Min = -32768/2;

// Initialize RMP3 module
	rmp3.DesiredInput = CmtnPeriodTarget;
	rmp3.Ramp3Delay = RampDelay;
    rmp3.Out = CmtnPeriodSetpt;
    rmp3.Ramp3Min = 0x00000010;

    InitVar();
//Call HVDMC Protection function
	HVDMC_Protection();

	EnableDog();
// IDLE loop. Just sit and loop forever:	
	for(;;)  //infinite loop
	{
		//BackTicker++;
		taskfree();
		// State machine entry & exit point
		//===========================================================
		//(*Alpha_State_Ptr)();	// jump to an Alpha state (A0,B0,...)
		//===========================================================

	}
} //END MAIN CODE



//=================================================================================
//	STATE-MACHINE SEQUENCING AND SYNCRONIZATION FOR SLOW BACKGROUND TASKS
//=================================================================================


// ==============================================================================
// =============================== MAIN ISR =====================================
// ==============================================================================

Uint32 IsrTicker=0;
interrupt void MainISR(void)

{

//--------------------------------------------------------------------------------------

// Verifying the ISR
    IsrTicker++;
    CanMaster();
    SerialCommsTimer++;
    if(SerialCommsTimer>0x3FFF){
        SerialCommsTimer = 0x3FFF;
    }

    ServiceDog();
    speed_cal();
    pwmlimit_speed();
    taskfree();
    FaultTreat();
#if (BUILDLEVEL==LEVEL1)

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP3 module and call the RAMP Control 3 macro.
// ------------------------------------------------------------------------------
      rmp3.DesiredInput = CmtnPeriodTarget;
      rmp3.Ramp3Delay = RampDelay;
      RC3_MACRO(&rmp3);

// ------------------------------------------------------------------------------
//    Connect inputs of the IMPULSE module and call the Impulse macro.
// ------------------------------------------------------------------------------
      impl1.Period = rmp3.Out;
      IMPULSE_MACRO(&impl1);
      HALL3_READ_MACRO(&hall1);
// ------------------------------------------------------------------------------
//    Connect inputs of the MOD6 module and call the Mod 6 counter macro.
// ------------------------------------------------------------------------------
      mod1.TrigInput = impl1.Out;
      MOD6CNT_MACRO(&mod1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PWM_DRV module and call the PWM signal generation
//    update macro.
// ------------------------------------------------------------------------------
      pwm1.CmtnPointerIn = test2;//(int16)mod1.Counter;
      if(EnableFlag == 0){
          pwm1.DutyFuncIn = 0;
      }else{
          pwm1.DutyFuncIn = test;
      }

      BLDCPWM_MACRO(1,2,3,&pwm1);


#endif // (BUILDLEVEL==LEVEL1)
// =============================== LEVEL 3 ======================================
//	  Level 3 describes the closed-loop operation of sensored trapezoidal 
//	  drive of BLDC motor using Hall sensor.
// ==============================================================================

#if (BUILDLEVEL==LEVEL3)

// ------------------------------------------------------------------------------
//    ADC conversion and offset adjustment (observing back-emfs is optinal for this prj.) 
// ------------------------------------------------------------------------------
	  BemfA =  _IQ12toIQ(AdcResult.ADCRESULT1);
	  BemfB =  _IQ12toIQ(AdcResult.ADCRESULT2); 
	  BemfC =  _IQ12toIQ(AdcResult.ADCRESULT3);
	  DCbus_current = _IQ12toIQ(AdcResult.ADCRESULT4);//-_IQ(0.5); //1.65V offset added on HVDMC board.

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP3 module and call the Ramp control 3 macro.
// ------------------------------------------------------------------------------


	 HALL3_READ_MACRO(&hall1);

	 ClosedFlag=TRUE;

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP2 module and call the Ramp control 2 macro.
// ------------------------------------------------------------------------------
      rmp2.DesiredInput = (int32)DFuncDesired;
	  RC2_MACRO(&rmp2);

// ------------------------------------------------------------------------------
//    Connect inputs of the PWM_DRV module and call the PWM signal generation
//    update macro.
// ------------------------------------------------------------------------------
   if (ClosedFlag==TRUE)  {
     if (hall1.CmtnTrigHall==0x7FFF) {

      PreviousState = pwm1.CmtnPointer;

// Comment the following if-else-if statements in case of 
// inverted Hall logics for commutation states. 
    /*  if (hall1.HallGpioAccepted==5)
      { pwm1.CmtnPointer = 0;

      }
      else if (hall1.HallGpioAccepted==1) 
      { pwm1.CmtnPointer = 1;

      }
      else if (hall1.HallGpioAccepted==3) 
      { pwm1.CmtnPointer = 2;

      }
      else if (hall1.HallGpioAccepted==2)
      { pwm1.CmtnPointer = 3;

      }
      else if (hall1.HallGpioAccepted==6)
      {  pwm1.CmtnPointer = 4;

      }
      else if (hall1.HallGpioAccepted==4) 
      {
          pwm1.CmtnPointer = 5;
      }*/




// Comment the following if-else-if statements in case of 
// non-inverted Hall logics for commutation states. 
      if (hall1.HallGpioAccepted==2) 
        pwm1.CmtnPointer = 0;

      else if (hall1.HallGpioAccepted==6) 
        pwm1.CmtnPointer = 1;

      else if (hall1.HallGpioAccepted==4) 
        pwm1.CmtnPointer = 2;

      else if (hall1.HallGpioAccepted==5)
        pwm1.CmtnPointer = 3;

      else if (hall1.HallGpioAccepted==1)
        pwm1.CmtnPointer = 4; 

      else if (hall1.HallGpioAccepted==3) 
        pwm1.CmtnPointer = 5;


    }    //hall1.CmtnTrigHall == 0x7FFF   
  } // ClosedFlag==TRUE
  else{
      pwm1.CmtnPointer = (int16)mod1.Counter;
  }
	  pwm1.DutyFunc = DfuncTesting;
	  BLDCPWM_MACRO(1,2,3,&pwm1);

// ------------------------------------------------------------------------------
//    Connect inputs of the SPEED_PR module and call the speed calculation macro.
// ------------------------------------------------------------------------------  
  if ((pwm1.CmtnPointer==5)&&(PreviousState==4)&&(hall1.CmtnTrigHall==0x7FFF))
  {
       speed1.TimeStamp = VirtualTimer;
	   SPEED_PR_MACRO(&speed1);
	   test  = (test ==0 )? 1 : 0;
  }

// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	   
      pwmdac1.MfuncC1 = (int32)hall1.HallGpioAccepted<<20; 
      pwmdac1.MfuncC2 = BemfA; 
      PWMDAC_MACRO(6,pwmdac1)	  						// PWMDAC 6A, 6B
    
      pwmdac1.MfuncC1 = DCbus_current; 
      pwmdac1.MfuncC2 = (int32)mod1.Counter<<20; 				
	  PWMDAC_MACRO(7,pwmdac1)	 						// PWMDAC 7A, 7B

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
      DlogCh2 = (int16)mod1.Counter; 
	  DlogCh1 = (int16)hall1.HallGpioAccepted;
      DlogCh3 = (int16)_IQtoIQ15(BemfA);
      DlogCh4 = (int16)_IQtoIQ15(BemfB);
      

#endif // (BUILDLEVEL==LEVEL3)

// =============================== LEVEL 4 ======================================
//	  Level 4 verifies the closed current loop and current PI controller.
// ==============================================================================
      //-------------平均电流和can-------------------------------
      static _iq last_DC_current=0;
       static Uint16 DC_Current_cnt = 0;
       //static int32 DC_Current_sum = 0;
       static int32 DC_Current_sum_filter = 0;

       DC_current_real = (Uint32)AdcResult.ADCRESULT5*33;//Q12  I真实 = adc * 3.3 / 4096 /0.1
       DC_current_filer = lowpassfilter(last_DC_current,DC_current_real,_IQ12(1),_IQ12(0.05));
       last_DC_current = DC_current_filer;
       carveData1();
       CanCurve();

       UserCANprocess();
       if(Fault_clear == 1) {
           static int16 fault_cnt = 0;
           GpioDataRegs.GPBCLEAR.bit.GPIO32 = 1;
           FaultFlag.bit.OverCurFlag = 0;
           fault_cnt++;
           if(fault_cnt>20000){
           Fault_clear = 0;
           fault_cnt = 0;

           }
       }else{
           GpioDataRegs.GPBSET.bit.GPIO32 = 1;
       }
       DC_Current_sum_filter += DC_current_filer*pwm1.DutyFunc>>15;
     //  DC_Current_sum += DC_current_real*pwm1.DutyFunc>>15;
       DC_Current_cnt++;//由于stall最多16384/20K秒 所以最多0.819715秒计算一次平均值



#if (BUILDLEVEL==LEVEL4) 
       BemfA =  AdcResult.ADCRESULT1;
       BemfB =  AdcResult.ADCRESULT2;
       BemfC =  AdcResult.ADCRESULT3;

	  HALL3_READ_MACRO(&hall1);

      if (hall1.Revolutions>=0)
         ClosedFlag=TRUE;
// ------------------------------------------------------------------------------
//    电流闭环
// ------------------------------------------------------------------------------  
      pid1_spd.Ref = _IQ(0.3);
      pid1_spd.Fbk = _IQabs(speed1.Speed);
     // tempIdc=pid1_idc.Fbk;
      pid1_idc.Umax = pwmlimit<<9;
      pid1_idc.Umin = (-pwmlimit)<<9;
      pid1_idc.Ref = CurrentSet;//给定电流 反转给-
      if(speed1.SpeedRpm<0){
          pid1_idc.Fbk = -_IQ12toIQ(DC_current_filter_avr);// 电流改为Q15格式
       }else{
           pid1_idc.Fbk = _IQ12toIQ(DC_current_filter_avr);// 电流改为Q15格式
      }
//	  if(pid1_idc.Fbk<0) pid1_idc.Fbk=tempIdc; // Eliminate negative values



      step++;
      if(step == 1){
            PI_MACRO(&pid1_idc);
       }else if(step == 2){
           PI_MACRO(&pid1_spd);
           step = 0;
       }

	  if(EnableFlag == 0){
	       SetPI(&pid1_spd);
           resetPI(&pid1_idc);
           rmp2.DesiredInput = 0;
           RC2_MACRO(&rmp2);
           pwm1.DutyFuncIn = (int16)_IQtoIQ15(rmp2.Out);   // controlled speed duty-cycle

	   }else{
	         pwm1.DutyFuncIn = (int16)_IQtoIQ15(_IQmpy(pid1_idc.Out,pid1_spd.Out));   // controlled speed duty-cycle
	         rmp2.Out = pwm1.DutyFuncIn;
	        // pwm1.DutyFuncIn = (int16)(_IQtoIQ15(pid1_idc.Out));
	   }


   	if (ClosedFlag==TRUE)  
   	{
     if (hall1.CmtnTrigHall==0x7FFF) 
     {
         DC_current_avr = DC_Current_sum/DC_Current_cnt;
         DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
         DC_Current_sum = 0;
         DC_Current_sum_filter =0;
         DC_Current_cnt = 0;
         //计算速度
         speed1.TimeStamp = VirtualTimer;
         SPEED_PR_MACRO(&speed1,hall1.HallGpioAccepted,PreviousState,hall1.Stall_status);
         PreviousState = hall1.HallGpioAccepted;
         if (hall1.HallGpioAccepted==2)
           pwm1.CmtnPointerIn = 0;

         else if (hall1.HallGpioAccepted==6)
           pwm1.CmtnPointerIn = 1;

         else if (hall1.HallGpioAccepted==4)
           pwm1.CmtnPointerIn = 2;

         else if (hall1.HallGpioAccepted==5)
           pwm1.CmtnPointerIn = 3;

         else if (hall1.HallGpioAccepted==1)
           pwm1.CmtnPointerIn = 4;

         else if (hall1.HallGpioAccepted==3)
         {     pwm1.CmtnPointerIn = 5;

         }//hall1.HallGpioAccepted==3
     }
  }

	 BLDCPWM_MACRO(1,2,3,&pwm1);
	  
#endif // (BUILDLEVEL==LEVEL4)


// =============================== LEVEL 5 ======================================
//	  Level 5 verifies the closed speed loop and speed PI controller.
// ============================================================================== 

#if (BUILDLEVEL==LEVEL5)

// ------------------------------------------------------------------------------
//    ADC conversion and offset adjustment (observing back-emfs is optinal for this prj.) 
// ------------------------------------------------------------------------------
	  BemfA =  AdcResult.ADCRESULT1;
	  BemfB =  AdcResult.ADCRESULT2;
	  BemfC =  AdcResult.ADCRESULT3;
	  //DCbus_current = _IQ12toIQ(AdcResult.ADCRESULT5); //采样值<<12

	  rc1.TargetValue = SpeedRef;
   	  RC_MACRO(&rc1);

      HALL3_READ_MACRO(&hall1);
      pid1_spd.Ref = rc1.SetpointValue;
      pid1_spd.Fbk = speed1.Speed;


	  if(EnableFlag == 0){
	      resetPI(&pid1_spd);
	      rmp2.DesiredInput = 0;
	      RC2_MACRO(&rmp2);
	      pwm1.DutyFuncIn = (int16)_IQtoIQ15(rmp2.Out);   // controlled speed duty-cycle

	  }else{
	      pid1_spd.Umax = _IQ((float)pwmlimit/32767);
	      pid1_spd.Umin = _IQ((float)(-pwmlimit)/32767);
	      PI_MACRO(&pid1_spd);
	      if((int16)_IQtoIQ15(pid1_spd.Out)>pwmlimit){
	          pwm1.DutyFuncIn = pwmlimit;   // controlled speed duty-cycle
	      }else if((int16)_IQtoIQ15(pid1_spd.Out)<-pwmlimit){
	          pwm1.DutyFuncIn = -pwmlimit;   // controlled speed duty-cycle
	      }else{
	          pwm1.DutyFuncIn = (int16)_IQtoIQ15(pid1_spd.Out);   // controlled speed duty-cycle
	      }
	      //pwm1.DutyFuncIn = test;
	      rmp2.Out = pwm1.DutyFuncIn;
	  }
	  if(pwm1.DutyFunc>0){
	      OpenPwm();
	  }else{
	       ClosePwm();
	  }

      ClosedFlag = TRUE;
   if (ClosedFlag==TRUE)  
   {

     if (hall1.CmtnTrigHall==0x7FFF) 
     {

    //  DC_current_avr = DC_Current_sum/DC_Current_cnt;
      DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
     // DC_Current_sum = 0;
      DC_Current_sum_filter =0;
      DC_Current_cnt = 0;
      speed_direction(hall1.HallGpioAccepted,PreviousState);

      if (hall1.HallGpioAccepted==5)
        { pwm1.CmtnPointerIn = 5;
         speed1.TimeStamp = VirtualTimer;

     //    SPEED_PR_MACRO(&speed1,hall1.HallGpioAccepted,PreviousState,hall1.Stall_status);

        }
        else if (hall1.HallGpioAccepted==1)
        { pwm1.CmtnPointerIn = 0;

        }
        else if (hall1.HallGpioAccepted==3)
        { pwm1.CmtnPointerIn = 1;

        }
        else if (hall1.HallGpioAccepted==2)
        { pwm1.CmtnPointerIn = 2;

        }
        else if (hall1.HallGpioAccepted==6)
        {  pwm1.CmtnPointerIn = 3;

        }
        else if (hall1.HallGpioAccepted==4)
        {
            pwm1.CmtnPointerIn = 4;
        }
      PreviousState = hall1.HallGpioAccepted;
      /*
          if (hall1.HallGpioAccepted==2)
            pwm1.CmtnPointerIn = 0;

          else if (hall1.HallGpioAccepted==6)
            pwm1.CmtnPointerIn = 1;

          else if (hall1.HallGpioAccepted==4)
            pwm1.CmtnPointerIn = 2;

          else if (hall1.HallGpioAccepted==5)
            pwm1.CmtnPointerIn = 3;

          else if (hall1.HallGpioAccepted==1)
            pwm1.CmtnPointerIn = 4;

          else if (hall1.HallGpioAccepted==3)
          {     pwm1.CmtnPointerIn = 5;

          }//hall1.HallGpioAccepted==3
*/
        //}//direction == Normal

    }    // delay
  }      // ClosedFlag==TRUE


	  BLDCPWM_MACRO(1,2,3,&pwm1);


// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
      pwmdac1.MfuncC1 = BemfA; 
      pwmdac1.MfuncC2 = BemfB; 
      PWMDAC_MACRO(6,pwmdac1)	  						// PWMDAC 6A, 6B
    
      pwmdac1.MfuncC1 = DCbus_current; 
      pwmdac1.MfuncC2 = mod1.Counter<<20; 				
	  PWMDAC_MACRO(7,pwmdac1)	 						// PWMDAC 7A, 7B

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------



#endif // (BUILDLEVEL==LEVEL5)


// ------------------------------------------------------------------------------
//    Call the DATALOG update function.
// ------------------------------------------------------------------------------


// ------------------------------------------------------------------------------
//    Increase virtual timer and force 15 bit wrap around
// ------------------------------------------------------------------------------
	VirtualTimer++;
	VirtualTimer &= 0x00007FFF;
   
	IsrTime = MastIsrTimePercent();
// Acknowledge interrupt to recieve more interrupts from PIE group 1
	CpuTimer0.RegsAddr->TCR.bit.TIF=1;
	PieCtrlRegs.PIEACK.bit.ACK1 = 1;
	//PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;

}// ISR Ends Here

/**********************************************************/
/***************Protection Configuration*******************/  
/**********************************************************/

void HVDMC_Protection(void)
{

// Configure Trip Mechanism for the Motor control software
// -Cycle by cycle trip on CPU halt
// -One shot IPM trip zone trip 
// These trips need to be repeated for EPWM1 ,2 & 3

//===========================================================================
//Motor Control Trip Config, EPwm1,2,3
//===========================================================================
      EALLOW;
// CPU Halt Trip  使能TZ6周期触发，周期触发可以自动清零 但是一次触发不能自动清零
  //    EPwm1Regs.TZSEL.bit.CBC6=0x1;//Enable TZ6 as a CBC trip source for this ePWM module
  //    EPwm2Regs.TZSEL.bit.CBC6=0x1;
  //    EPwm3Regs.TZSEL.bit.CBC6=0x1;
      
      //使能故障的单次触发
      EPwm1Regs.TZSEL.bit.OSHT1   = 1;  //enable TZ1 for OSHT
      EPwm2Regs.TZSEL.bit.OSHT1   = 1;  //enable TZ1 for OSHT
      EPwm3Regs.TZSEL.bit.OSHT1   = 1;  //enable TZ1 for OSHT

      EPwm1Regs.TZEINT.bit.OST = 1;

// What do we want the OST/CBC events to do?
// TZA events can force EPWMxA
// TZB events can force EPWMxB
//故障发生时的输出状态为低
      EPwm1Regs.TZCTL.bit.TZA = TZ_FORCE_LO; // EPWMxA will go low 
      EPwm1Regs.TZCTL.bit.TZB = TZ_FORCE_LO; // EPWMxB will go low
      
      EPwm2Regs.TZCTL.bit.TZA = TZ_FORCE_LO; // EPWMxA will go low 
      EPwm2Regs.TZCTL.bit.TZB = TZ_FORCE_LO; // EPWMxB will go low
      
      EPwm3Regs.TZCTL.bit.TZA = TZ_FORCE_LO; // EPWMxA will go low 
      EPwm3Regs.TZCTL.bit.TZB = TZ_FORCE_LO; // EPWMxB will go low
      
      
      EDIS;

     // Clear any spurious OV trip 清除标志位
      EPwm1Regs.TZCLR.bit.OST = 1;
      EPwm2Regs.TZCLR.bit.OST = 1;
      EPwm3Regs.TZCLR.bit.OST = 1;  
      
//************************** End of Prot. Conf. ***************************//
}
//速度计算中断
interrupt void xint2_isr(void)
{
    static int32 last_XintTime = TIMER1_PER;

   if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){
        // 速度为0
        XintTime = (int32)TIMER1_PER;
        CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清中断标志位
    }else{
        XintTime = (int32)TIMER1_PER - CpuTimer1.RegsAddr->TIM.all;
    }

   if(XintTime<30000){
       //filter 防抖
       XintTime = last_XintTime;
   }
   last_XintTime = XintTime;

   if(Direction == -1){
       XintTime = -XintTime;
   }
   Xint2Cnt = 1;
    CpuTimer1.RegsAddr->TCR.bit.TRB = 1; //重新装载
    CpuTimer1.RegsAddr->TCR.bit.TSS = 0;//定时器restart

    PieCtrlRegs.PIEACK.bit.ACK1 =1;


}

interrupt void xint1_isr(void)
{

    FaultFlag.bit.IgbtTempFaultFlag = 1;
    PieCtrlRegs.PIEACK.bit.ACK1 =1;

   // PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;

}
interrupt void EPWM1TZint_isr(void)
{
//過流
    FaultFlag.bit.OverCurFlag = 1;
    EPwm1Regs.TZCLR.bit.OST = 1;
    EPwm1Regs.TZCLR.bit.INT = 1;
    PieCtrlRegs.PIEACK.bit.ACK2 = 1;//PIEACK_GROUP2;
}
void ServiceDog(void)
{
    EALLOW;
    SysCtrlRegs.WDKEY = 0x0055;
    SysCtrlRegs.WDKEY = 0x00AA;
    EDIS;
}

//---------------------------------------------------------------------------
// Example: DisableDog:
//---------------------------------------------------------------------------
// This function disables the watchdog timer.


//---------------------------------------------------------------------------
// 使能看门狗
//---------------------------------------------------------------------------
void EnableDog(void)
{
    EALLOW;
    SysCtrlRegs.WDCR= 0x0028;
    EDIS;
}

void OpenPwm(void)
{
    EALLOW;

    EPwm1Regs.TZCLR.bit.OST = 1;     //EnablePWM1

    EPwm2Regs.TZCLR.bit.OST = 1;     //EnablePWM2

    EPwm3Regs.TZCLR.bit.OST = 1;     //EnablePWM3

    EPwm1Regs.TZCLR.bit.INT = 1;
    EDIS;
}
void ClosePwm(void)
{
    EALLOW;

    EPwm1Regs.TZFRC.bit.OST = 0x1;        //disabPWMl

    EPwm2Regs.TZFRC.bit.OST = 0x1;        //disabPWM2

    EPwm3Regs.TZFRC.bit.OST = 0x1;        //disabPWM3

    EDIS;
}
void speed_direction(Uint16 hall,Uint16 hall_last){


    if(hall_last == 2 && hall == 6 || hall_last == 6 && hall == 4 ||hall_last == 4 && hall == 5 ||hall_last == 5 && hall == 1||hall_last == 1 && hall == 3||hall_last == 3 && hall == 2)
    {
        Direction = 1;
    }
     else if(hall_last == 4 && hall == 6 || hall_last == 6 && hall == 2 ||hall_last == 2 && hall == 3 ||hall_last == 3 && hall == 1||hall_last == 1 && hall == 5|| hall_last == 5 && hall == 4)
     {
         Direction = -1;
     }

}
void speed_cal(void){
  /*  int32 RotarfreOut = 0;
     int32 RotarfreqNew = 0;//电周期频率

     static int32 RotarfreSum = 0;
     static int32 RotarfreqArr[8] = {0,0,0,0,0,0,0,0};
     static int16 j = 0;*/
    int32 RotarRPMNew = 0;
     int32 RotarrpmOut = 0;
    static int32 RotarrpmSum = 0;
    static int32 RotarrpmArr[16] = {0,0,0,0,0,0,0,0,0,0,
                              0,0,0,0,0,0};

    static int16 k = 0;
    if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){
         // 速度为0
         XintTime = TIMER1_PER;
         Xint2Cnt = 1;
         CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清零
        // RotarfreqNew =0;
         RotarRPMNew =0;
     }else{
         //RotarfreqNew = (int32)(30000000)/XintTime;//60M clock /N
         RotarRPMNew =(int32)360000000/XintTime;
     }
     if(Xint2Cnt == 1){
         Xint2Cnt = 0;
         /* RotarfreqArr[j] = RotarfreqNew;
         RotarfreSum += RotarfreqArr[j];
         RotarfreOut = RotarfreSum >> 3;
         j++;
         if (j >= 8)
         {
             j = 0;
         }
         RotarfreSum -= RotarfreqArr[j];


         speed1.Speed = _IQdiv(RotarfreOut,BASE_FREQ);
         speed1.SpeedRpm = _IQmpy(speed1.BaseRpm,speed1.Speed);
          */


         RotarrpmArr[k] = RotarRPMNew;
         RotarrpmSum += RotarrpmArr[k];
         RotarrpmOut = RotarrpmSum>>4;
         k++;
         if (k >= 16)
         {
             k = 0;
         }
         RotarrpmSum -= RotarrpmArr[k];
         speed1.Speed = _IQdiv(RotarrpmOut,speed1.BaseRpm);
         speed1.SpeedRpm = RotarrpmOut;
     }
}
void pwmlimit_speed(void){
    if(_IQabs(speed1.SpeedRpm)< 200){
        Uint16 index = _IQabs(speed1.SpeedRpm)/50;
        pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*50)*(pwm_limit_table1[index+1] - pwm_limit_table1[index])/50+pwm_limit_table1[index];
    }else  if(_IQabs(speed1.SpeedRpm)< 3500){
        Uint16 index = _IQabs(speed1.SpeedRpm)/100;
        pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*100)*(pwm_limit_table2[index+1] - pwm_limit_table2[index])/100+pwm_limit_table2[index];

    }else{
        pwmlimit = 25240;//最大值
    }
}
void taskfree(void){


    if(SerialCommsTimer>4000){
    GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1;

    SerialCommsTimer = 0;

    DCbus_voltage =  AdcResult.ADCRESULT4/10;// U = Uadc * 0.099964


    _iq20 kb = _IQ20(18.26);
    Tvot = ((Uint32)AdcResult.ADCRESULT7 *kscaler  - kb)>>20;
    int32 R = 10000;//分压电阻
    int32 Rtadc = AdcResult.ADCRESULT6;
    Rt= Rtadc*R/(4096-Rtadc);

    if(Rt>32116){
        Tmotor = 0;
    }
    else if(Rt>15652){

        Tmotor = (_IQ12(-0.901)*Rt/1000 + _IQ12(28.381))>>12;

    }else if(Rt>6523){
        Tmotor = (_IQ12(-2.5013)*Rt/1000 + _IQ12(50.657))>>12;

    }else if(Rt>2968){

        Tmotor = (_IQ12(-6.3342)*Rt/1000 + _IQ12(73.203))>>12;

    }else if(Rt>1228){
        Tmotor = (_IQ12(-16.002)*Rt/1000 + _IQ12(98.576))>>12;

    }else if(Rt > 657){
        Tmotor = (_IQ12(-38.551)*Rt/1000 + _IQ12(124.83))>>12;

    }else if(Rt > 324){
        Tmotor = (_IQ12(-81.812)*Rt/1000 + _IQ12(150.67))>>12;

    }else{
        Tmotor = 125;
    }
   }

}
int16 MastIsrTimePercent(void)
{
    int16 MasterIsrT = 0;


    MasterIsrT = 3000 - CpuTimer0.RegsAddr->TIM.all;

    MasterIsrT = (Uint32)MasterIsrT* 100 /3000;


    return MasterIsrT;
}