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才能启动起来
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};*/
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};
/*Uint16 pwm_limit_table_neg[]= {950,1650,2320,3050,3750,4476,5200,5830,6520,7200,7808,
                           8500,9206,9800,10510,11200,11930,12579,13229,13892,14540,
                           15209,15920,16616,17291,18000,18775,19424,20507,21260,21926,
                           22602,23232,23935,24609,25240};*/
int32 pwmlimitrate = 24507;

Uint32 IsrTicker=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)
{

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

// Verifying the ISR
    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)<30000 || temp == last_hall ){
       //filter 防抖
       XintTime = last_XintTime;
       PieCtrlRegs.PIEACK.bit.ACK1 =1;

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

    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_2_cnt++;
             if(Hall_2_cnt>MAINLOOPFREQ/2){//0.5s
                 Hall_2_cnt = MAINLOOPFREQ/2+1;
             }
         }else{
             Hall_2_cnt = 0;
         }
         if(XintTime == TIMER1_PER || Hall_2_cnt>=5000){
             RotarRPMNew = 0;
             last_speed = 0;
         }else{
         RotarRPMNew =(int32)360000000/XintTime;
         }
     }
    test2 = RotarRPMNew;
     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;
         test4 = Halltemp;

     }



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

         }

      /*   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.1),_IQ12(0.125));
         last_speed = RotarrpmOut;

         speed1.Speed = _IQdiv(RotarrpmOut,speed1.BaseRpm);
         speed1.SpeedRpm = RotarrpmOut;
  //   }
}
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(_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;
        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(speed1.SpeedRpm>0){
            pwmlimit = pwm_limit_table2[36];//最大值
        }else{
        pwmlimit = pwm_limit_table_neg[36];//最大值
        }
    }
}
void taskfree(void){


    if(SerialCommsTimer>(MAINLOOPFREQ/4)){
    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)
{
    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;
     }

     ServiceDog();

     //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;
        }
        carveData1();
        CanCurve();
        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;


       pid1_spd.Ref = SpeedRef;//rc1.SetpointValue;
       pid1_spd.Fbk = speed1.Speed;
       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{
           pid1_spd.Umax = _IQ((float)pwmlimit/32767);
           pid1_spd.Umin = _IQ((float)(-pwmlimit)/32767);

           PI_MACRO(&pid1_spd);
           int16 pwmtmp =(int16)_IQtoIQ15(pid1_spd.Out);

           //pwmtmp = test;//上位机开环控制
           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)
       //程序占用率
       //IsrTime = MastIsrTimePercent();
       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    */

}