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_S1 60
#define TIMER1_S2 500000
#define TIMER1_PER 30000000
#define MAINLOOPDIV 5 // 主中断40K 40K/MAINLOOPDIV 主循环
// 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 bldcpwm_close(void);
void OpenPwm(void);
void mainLoop(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 t4 = 0;
int16 Xint2Cnt = 0;
int16 hallCmtnTrig = 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才能启动起来
/*//50N
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,2985,3668,4348,5024,5705,6383,7061,7760,
                           8463,9141,9820,10510,11187,11859,12684,13216,13895,14558,//1100-2000
                           15280,15955,16635,17290,17970,18645,19320,19960,20635,21299,
                           21970,22640,23280,24000,24700,24700};
Uint16 pwm_limit_table_neg[]= {950,1616,2200,2930,3614,4286,4961,5639,6313,6987,7670,
                               8348,9022,9700,10361,11080,11714,12397,13080,13743,14438,//1100-2000
                               15117,15730,16425,17092,17757,18434,19100,19781,20425,21081,
                               21743,22428,23097,23781,24402,24402};*/
/*//55N    后来测试为50N

Uint16 pwm_limit_table1[5] = {950,1330,1650,2000,2300};//0 50 100 150 200

Uint16 pwm_limit_table1_NEG[5] = {950,1300,1650,1935,2285};

Uint16 pwm_limit_table2[]= {950,1650,2300,3030,3750,4455,5130,5830,6515,7210,7900,
                           8580,9268,9990,10667,11363,12045,12725,13420,14120,14800,//1100-2000
                           15500,16196,16875,17555,18120,18800,19450,20175,20850,21665,
                           22320,22950,23610,24260,24945,24945};
Uint16 pwm_limit_table_neg[]= {950,1300,2290,2980,3700,4370,5070,5770,6420,7120,7800,
                               8500,9180,9860,10540,11235,11910,12570,13260,13945,14600,//1100-2000
                               15280,15940,16615,17290,17925,18600,19280,19950,20610,21260,
                               21930,22550,23230,23890,24560,24560};*/
//first test ----------
/*Uint16 pwm_limit_table2[]= {950,1650,2320,2985,3668,4348,5024,5705,6383,7058,7739,
                           8449,9122,9800,10480,11163,11826,12499,13181,13860,14515,
                           15192,15872,16530,17203,17861,18537,19183,19848,20525,21161,
                           21838,22488,23216,23850,24507,24507};*/
    ////                     0,    100,  200, 300, 400 500 600  700   800  900 1000
/*Uint16 pwm_limit_table_neg[]= {950,1616,2272,2961,3641,4305,4983,5659,6326,6994,7670,
                               8348,9022,9700,10361,11502,11714,12379,13066,13727,14426,//1100-2000
                               15085,15610,16325,17005,17660,18338,19000,19693,20390,21048,
                               21706,22428,23104,22823,24434,24434};*/

//first test ----------
//---55Nm_2-----------------------------------------------------------
Uint16 pwm_limit_table1[5] = {1000,1340,1680,2020,2360};//0 200

Uint16 pwm_limit_table1_NEG[5] = {1000,1350,1740,2070,2400};//0 200

Uint16 pwm_limit_table2[]= {1000,1680,2360,3080,3780,4480,5200,5920,6630,7345,8045,
                           8765,9480,10160,10860,11560,12280,13000,13680,14400,15040,//1100-2000
                           15740,16440,17110,17830,18510,19210,19910,20610,21260,21950,
                           22600,23300,23990,24630,25320,25320};
Uint16 pwm_limit_table_neg[]= {1000,1740,2400,3100,3800,4480,5190,5900,6600,7300,8000,
                               8700,9400,10100,10790,11470,12170,12870,13570,14260,14970,//1100-2000
                               15620,16300,17000,17660,18340,19038,19715,20395,21070,21750,
                               22400,23070,23760,24420,25100,25100};
//---59Nm_2-----------------------------------------------------------
/*Uint16 pwm_limit_table1[5] = {1000,2440};//0 200

//Uint16 pwm_limit_table1_NEG[5] = {1000,2500};//0 200

Uint16 pwm_limit_table2[]= {1000,1720,2440,3190,3890,4560,5290,6000,6730,7430,8150,
                           8835,9550,10260,10990,11690,12410,13100,13830,14530,15210,//1100-2000
                           15930,16610,17310,18000,18660,19360,20000,20740,21430,22100,
                           22810,23500,24150,24800,25320,25320};
Uint16 pwm_limit_table_neg[]= {1000,1850,2500,3210,3890,4590,5290,5990,6690,7400,8110,
                               8800,9480,10180,10880,11560,12260,12950,13640,14350,15040,//1100-2000
                               15730,16390,17090,17770,18470,19120,19820,20490,21180,21800,
                               22470,23160,23800,24420,25100,25100};*/
//------------------------------------------------------------------
int32 pwmlimitrate = 24507;
//NTC
_iq20 kb = _IQ20(18.26);
_iq12 ktable[] = {_IQ12(-0.901),_IQ12(-2.5013),_IQ12(-6.3342),_IQ12(-16.002),_IQ12(-38.551),_IQ12(-81.812)};
_iq12 btable[] = {_IQ12(28.381),_IQ12(50.657),_IQ12(73.203),_IQ12(98.576),_IQ12(124.83),_IQ12(150.67)};
Uint32 IsrTicker=0;
Uint32 startuptimer=0;
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_S1, TIMER1_S2);//2hz
   ConfigCpuTimer(&CpuTimer2, 60, 1000000);//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 = 0;                 // 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 = 1;
    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
	{
	    if(IsrTicker%MAINLOOPDIV ==0)
	    {//8Khz
	        mainLoop();
	    }
	}
} //END MAIN CODE



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


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


interrupt void MainISR(void)
{

//--------------------------------------------------------------------------------------
    ServiceDog();

// Verifying the ISR
    startuptimer++;
    startuptimer = (startuptimer>60000)? 60000 : startuptimer;//1.5S

    IsrTicker++;
    IsrTicker = (IsrTicker>20000)? 0 : IsrTicker;
    VirtualTimer++;
    VirtualTimer &= 0x00007FFF;
    HALL3_READ_MACRO(&hall1);
    ClosedFlag = TRUE;
  if (ClosedFlag==TRUE)
  {

    if (hall1.CmtnTrigHall==0x7FFF)
    {

        hallCmtnTrig = 1;
        speed_direction(hall1.HallGpioAccepted,PreviousState);

     if (hall1.HallGpioAccepted==5)
       {
         pwm1.CmtnPointerIn = 5;
       }
       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;


   }    // delay
 }      // ClosedFlag==TRUE



     if(Enable_ALLOW == 0 && pwm1.DutyFunc==0){
         bldcpwm_close();


      }else{
          BLDCPWM_MACRO(1,2,3,&pwm1);

      }
// 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;
    static Uint32 last_hall = 0;
    Uint32 temp = 0;
  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;
    }
    //XintTime = (int32)TIMER1_PER - CpuTimer1.RegsAddr->TIM.all;
    temp = (GpioDataRegs.GPADAT.all>>24)&0x00000007; /* read all three GPIOs at once*/

   if(_IQabs(XintTime)<36000 || temp == last_hall ){// 不应该超过10000rpm 也不应该和上一次的hall相同 否则说明有振动
       //filter 防抖
       XintTime = XintTime + last_XintTime;
       PieCtrlRegs.PIEACK.bit.ACK1 =1;
       last_XintTime = XintTime;
       last_hall = temp;

       if(Direction == -1){
           XintTime = -XintTime;
       }
       return ;
   }
   last_XintTime = XintTime;
   last_hall = temp;

   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.Ipmfault = 1;
    PieCtrlRegs.PIEACK.bit.ACK1 =1;

   // PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;

}
interrupt void EPWM1TZint_isr(void)
{
//過流
    FaultFlag.bit.OverCurFlag = 1;


    EALLOW;

   // EPwm1Regs.TZCLR.bit.OST = 1;
   // EPwm1Regs.TZCLR.bit.INT = 1;
    EDIS;
    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){
//防抖
    static int16 i = 0;
    static int16 j=0;

    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)
    {
        i++;
        j--;
        if(j<0){
            j=0;
        }
        if(i>5){
            i = 6;
            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)
     {
         i--;
         if(i<0){
             i=0;
         }
         j++;
         if(j>5){
             j= 6;
             Direction = -1;
         }

     }

}
void speed_cal_filter(void){
    int32 RotarRPMNew = 0;
     int32 RotarrpmOut = 0;
    static int32 last_speed = 0;
    static int32 last_RotarRPMNew = 0;
    Uint16 Halltemp = (Uint16)((GpioDataRegs.GPADAT.all>>24)&0x00000007); /* read all three GPIOs at once*/
     static Uint16 Hall_1 = 0;
     //static Uint16 Hall_2 = 0;
     static int32 Hall_2_cnt = 0;
     int16 directiontemp = 0;

    if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){
         // 速度为0
         XintTime = TIMER1_PER;
         Xint2Cnt = 1;
         RotarRPMNew =0;
         last_speed = 0;
         CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清零


     }else{
         if(Halltemp == hall1.HallGpioAccepted){//hall读取不变
             Hall_2_cnt++;
             if(Hall_2_cnt>MAINLOOPFREQ/2){//0.5s
                 Hall_2_cnt = MAINLOOPFREQ/2+1;
             }
         }else{
             Hall_2_cnt = 0;
         }
         if(_IQabs(XintTime) == TIMER1_PER || Hall_2_cnt>=4000){
             RotarRPMNew = 0;
             last_speed = 0;
         }else{
         RotarRPMNew =(int32)360000000/XintTime;//计算新速度
         }
     }

     if(Xint2Cnt == 1){
         //防止突然反向
         Xint2Cnt = 0;
              if(Hall_1 == 2 && Halltemp == 6 || Hall_1 == 6 && Halltemp == 4 ||Hall_1 == 4 && Halltemp == 5 ||Hall_1 == 5 && Halltemp == 1||Hall_1 == 1 && Halltemp == 3||Hall_1 == 3 && Halltemp == 2)
          {
             directiontemp = 1;
          }
          else if(Hall_1 == 4 && Halltemp == 6 || Hall_1 == 6 && Halltemp == 2 ||Hall_1 == 2 && Halltemp == 3 ||Hall_1 == 3 && Halltemp == 1||Hall_1 == 1 && Halltemp == 5|| Hall_1 == 5 && Halltemp == 4)
          {

              directiontemp = -1;
          }
         if((Direction ==1 && directiontemp == -1 )||(Direction == -1 && directiontemp == 1 )){
             //说明在振动
             RotarRPMNew = 0;
             last_speed = 0;
         }
         Hall_1 = Halltemp;


     }

             test2 = RotarRPMNew;

         if(_IQabs(RotarRPMNew - last_RotarRPMNew)>1500 &&(_IQabs(RotarRPMNew)<300 ||_IQabs(last_RotarRPMNew)<300) ){
             //在0转速附近有大的阶跃
             test = RotarRPMNew;
             RotarRPMNew = 0;
             last_speed = 0;
             last_RotarRPMNew = 0;

         }else if(_IQabs(RotarRPMNew - last_RotarRPMNew)>1500&&(_IQabs(RotarRPMNew)<1500 ||_IQabs(last_RotarRPMNew)<1500)){
            //低转速附近有大的阶跃
             if(_IQabs(RotarRPMNew)>_IQabs(last_RotarRPMNew)){
                 test3 = RotarRPMNew;
                 test5 = last_RotarRPMNew;
                 RotarRPMNew = last_RotarRPMNew;
             }
         }

      /*   if(RotarRPMNew - last_RotarRPMNew>500){
             RotarRPMNew = last_RotarRPMNew+4;
         }else if(RotarRPMNew - last_RotarRPMNew<-500){
             RotarRPMNew = last_RotarRPMNew-4;
         }*/
         //last_RotarRPMNew = RotarRPMNew;
         RotarrpmOut = lowpassfilter(last_speed,RotarRPMNew,_IQ12(0.584),_IQ12(0.125));
         last_speed = RotarrpmOut;

         speed1.Speed = _IQdiv(RotarrpmOut,speed1.BaseRpm);
         speed1.SpeedRpm = RotarrpmOut;
         last_RotarRPMNew = speed1.SpeedRpm;
  //   }
}
void speed_cal(void){

    int32 RotarRPMNew = 0;
     int32 RotarrpmOut = 0;
    static int32 RotarrpmSum = 0;
    static int32 RotarrpmArr[8] = {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清零
         int count = 0;
         for(count = 0;count<8;count++){
                RotarrpmArr[count] = 0;
            }
         RotarrpmSum = 0;
         RotarRPMNew =0;
     }else{
         RotarRPMNew =(int32)360000000/XintTime;
     }
     if(Xint2Cnt == 1){
         Xint2Cnt = 0;
         RotarrpmArr[k] = RotarRPMNew;
         RotarrpmSum += RotarrpmArr[k];
         RotarrpmOut = RotarrpmSum>>3;
         k++;
         if (k >= 8)
         {
             k = 0;
         }
         RotarrpmSum -= RotarrpmArr[k];
         speed1.Speed = _IQdiv(RotarrpmOut,speed1.BaseRpm);
         speed1.SpeedRpm = RotarrpmOut;
     }
}
void pwmlimit_speed(void){
    if(Enable_ALLOW == FALSE){
            if(_IQabs(speed1.SpeedRpm)< 200){
                startuptimer = 0;
                pwmlimit = 1300;
            }
        }
    if(_IQabs(speed1.SpeedRpm)< 200 && startuptimer>=60000){
        Uint16 index = _IQabs(speed1.SpeedRpm)/50;
        if(speed1.SpeedRpm>0){
            pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*50)*(pwm_limit_table1[index+1] - pwm_limit_table1[index])/50+pwm_limit_table1[index];
        }else{
            pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*50)*(pwm_limit_table1_NEG[index+1] - pwm_limit_table1_NEG[index])/50+pwm_limit_table1_NEG[index];

        }
    }else  if(_IQabs(speed1.SpeedRpm)< 3500 && startuptimer>=60000){
        Uint16 index = _IQabs(speed1.SpeedRpm)/100;
        if(speed1.SpeedRpm>0){
            pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*100)*(pwm_limit_table2[index+1] - pwm_limit_table2[index])/100+pwm_limit_table2[index];
        }else{
            pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*100)*(pwm_limit_table_neg[index+1] - pwm_limit_table_neg[index])/100+pwm_limit_table_neg[index];
        }

    }else if(startuptimer>=60000){
        if(speed1.SpeedRpm>0){
            pwmlimit = pwm_limit_table2[36];//最大值
        }else{
        pwmlimit = pwm_limit_table_neg[36];//最大值
        }
    }else{

    }

}
void taskfree(void){


    if(SerialCommsTimer>(MAINLOOPFREQ/4)){
    GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1;

    SerialCommsTimer = 0;

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



    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 = (ktable[0]*Rt/1000 + btable[0])>>12;

    }else if(Rt>6523){
        Tmotor = (ktable[1]*Rt/1000 + btable[1])>>12;

    }else if(Rt>2968){

        Tmotor = (ktable[2]*Rt/1000 + btable[2])>>12;

    }else if(Rt>1228){
        Tmotor = (ktable[3]*Rt/1000 + btable[3])>>12;

    }else if(Rt > 657){
        Tmotor = (ktable[4]*Rt/1000 + btable[4])>>12;

    }else if(Rt > 324){
        Tmotor = (ktable[5]*Rt/1000 + btable[5])>>12;

    }else{
        Tmotor = 125;
    }
   }

}
int16 MastIsrTimePercent(void)
{
    Uint32 T1 = 0;

    int16 MasterIsrT =0;
    static int32 T1_L = 60000000;

    T1 = CpuTimer2.RegsAddr->TIM.all;
    if(T1_L < T1){
        MasterIsrT = (Uint32)(T1_L +CpuTimer2.RegsAddr->PRD.all -T1)* 100 /9000;
    }else{
        MasterIsrT = (Uint32)(T1_L-T1)* 100 /9000;
    }

    T1_L = T1;
   // MasterIsrT = (Uint32)MasterIsrT* 100 /3000;


    return MasterIsrT;
}

void mainLoop(void){
    Uint32 T1 = 0;
    Uint32 T2 = 0;
    T1 = CpuTimer2.RegsAddr->TIM.all;
    CanMaster();
     SerialCommsTimer++;
     if(SerialCommsTimer>0x3FFF){
         SerialCommsTimer = 0x3FFF;
     }



     //speed_cal();
     speed_cal_filter();
     pwmlimit_speed();
     taskfree();
     FaultTreat();


 // =============================== 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;
       int32 dt = _IQ12(0.1);
       int32 cutoff = _IQ12(1);
        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,cutoff,dt);
        last_DC_current = DC_current_filer;
        DC_Current_sum_filter += DC_current_filer*pwm1.DutyFunc>>15;
        DC_Current_cnt++;//由于stall最多16384/20K秒 所以最多0.819715秒计算一次平均值
        if(hallCmtnTrig==1){
            hallCmtnTrig = 0;
            DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
            DC_Current_sum_filter =0;
            DC_Current_cnt = 0;
        }
#ifdef DEBUG_CAN
        carveData1();
        CanCurve();
#endif
        UserCANprocess();

 #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;

#ifdef SPEEDCLOSED
       pid1_spd.Ref = SpeedRef;//rc1.SetpointValue;
       pid1_spd.Fbk = speed1.Speed;
#endif
       if(EnableFlag == 0 || FaultFlag.all !=0){
           Enable_ALLOW = FALSE;
       }else{
           Enable_ALLOW = TRUE;
       }

       if(Enable_ALLOW == FALSE){

           resetPI(&pid1_spd);

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

       }else{
           int16 pwmtmp = 0;

           pid1_spd.Umax = _IQ15toIQ(pwmlimit);//_IQ((float)pwmlimit/32767);
           pid1_spd.Umin = -pid1_spd.Umax;//_IQ((float)(-pwmlimit)/32767);
           rmp2.Ramp2Max = pwmlimit;
           rmp2.Ramp2Min = -pwmlimit;
#ifdef SPEEDCLOSED
           PI_MACRO(&pid1_spd);
           pwmtmp =(int16)_IQtoIQ15(pid1_spd.Out);
           rmp2.DesiredInput = pwmtmp;
#else
           rmp2.DesiredInput = PwmSet;
#endif


           RC2_MACRO(&rmp2);
           pwmtmp = rmp2.Out;
           pwm1.DutyFuncIn = ((int32)pwmtmp*LimitMotCtrTemp(600000))>>15;//1分钟降一次

           if(pwm1.DutyFuncIn>pwmlimit){
               pwm1.DutyFuncIn = pwmlimit;   // controlled speed duty-cycle
           }else if(pwm1.DutyFuncIn<-pwmlimit){
               pwm1.DutyFuncIn = -pwmlimit;   // controlled speed duty-cycle
           }

           //rmp2.Out = pwm1.DutyFuncIn;
       }


 #endif // (BUILDLEVEL==LEVEL5)
       //程序占用率

       T2 = CpuTimer2.RegsAddr->TIM.all;
       if(T1 < T2){
           IsrTime = (Uint32)(T1 +CpuTimer2.RegsAddr->PRD.all -T2)* 100 /7500;
       }else{
          IsrTime = (Uint32)(T1-T2)* 100 /7500;
       }

}
void bldcpwm_close(void){
    EPwm1Regs.AQCSFRC.bit.CSFA = 1;            /* Forcing a continuous Low on output A of EPWM3    */
    EPwm1Regs.AQCSFRC.bit.CSFB = 1;            /* Forcing a continuous Low on output B of EPWM3    */

    EPwm2Regs.AQCSFRC.bit.CSFA = 1;            /* Forcing a continuous Low on output A of EPWM3    */
    EPwm2Regs.AQCSFRC.bit.CSFB = 1;            /* Forcing a continuous Low on output B of EPWM3    */

    EPwm3Regs.AQCSFRC.bit.CSFA = 1;            /* Forcing a continuous Low on output A of EPWM3    */
    EPwm3Regs.AQCSFRC.bit.CSFB = 1;            /* Forcing a continuous Low on output B of EPWM3    */

}