speedpid.c

2812例程以及 控制直流无刷电机程序

// Include header files used in the main function
#include "target.h"

#include "DSP281x_Device.h"

#include "IQmathLib.h"
#include "bldc3_1.h"
#include "parameter.h"
#include "build.h"
#include <math.h>
//#include "ICETEK_EMPC.h"
//#include "EMPC_STS_10k.h"
#include "EMPC_STS_5k.h"
//#pragma DATA_SECTION(EMPC_spd,"mpt_data")
//ICETEK_EMPC EMPC_spd = ICETEK_EMPC_DEFAULTS;

// Prototype statements for functions found within this file.
interrupt void MainISR(void);

// Create an instance of the ADC driver
long lnADCValues;
int16 nADCValue;
float32 fSpeedSet=0.2,fWork;
unsigned int uWork=0;
int nWork=0,nWork1;
int nSpeedRef=0,nSpeed=0,nCurrent=0;
float fAdjust=0;

// Global variables used in this system
float32 SpeedRef = 0.20;           // Speed reference (pu)
float32 T = 0.001/ISR_FREQUENCY;   // Samping period (sec), see parameter.h

Uint32 VirtualTimer = 0;
Uint16 ILoopFlag = FALSE;
Uint16 SpeedLoopFlag = FALSE;
int16 DFuncDesired = 0x1A00;      // Desired duty cycle (Q15)

_iq CurrentSet = _IQ(0.0031);

Uint16 IsrTicker = 0;
Uint16 BackTicker = 0;

int16 DlogCh1 = 0;
int16 DlogCh2 = 0;
int16 DlogCh3 = 0;
int16 DlogCh4 = 0;

Uint16 uISRWork;
int16 nISRWork;
int16 nISRWork1;
int16 nLastStatus,nNowStatus;
Uint16 uISRWork1=3300;
Uint16 uISRWork2;
Uint16 bISRWork;		

int16 nHallABC[6]={ 4,0,5,2,3,1 };

volatile Uint16 EnableFlag = FALSE;

// Instance PID regulator to regulate the DC-bus current and speed
//PIDREG3 pid1_idc = PIDREG3_DEFAULTS;
//PIDREG3 pid1_spd = PIDREG3_DEFAULTS;
ICETEK_PIDA pid2_spd = ICETEK_PIDA_DEFAULTS;

// Instance a PWM driver instance
//PWMGEN pwm1 = PWMGEN_DEFAULTS;

// Create an instance of the ADC driver
//ADCVALS adc1 = ADCVALS_DEFAULTS;

// Instance a Hall effect driver
HALL3 hall1 = HALL3_DEFAULTS;
/*
// Instance a ramp controller to smoothly ramp the frequency
RMPCNTL rc1 = RMPCNTL_DEFAULTS;

// Instance a RAMP2 Module
RMP2 rmp2 = RMP2_DEFAULTS;
*/
// Instance a MOD6 Module
MOD6CNT mod1 = MOD6CNT_DEFAULTS;

// Instance a SPEED_PR Module
SPEED_MEAS_CAP speed1 = SPEED_MEAS_CAP_DEFAULTS;

///////////////////////////////////////////////////////////
int16 vadc1,vadc2,vadc3,vadc4;
int16 vadc1_1,vadc2_1;
float iadc1,iadc2;
float iadc3,te,wiz;
Uint16 empc_flag;
int32 speed_count;

float X0[3];
Uint16 U_pwm;

_iq27 the_r_base , d_the_rp , d_the_r, the_r;
int16 counter_the_r = 0;

#define CPU_CLOCK_SPEED      6.6667L   // for a 150MHz CPU clock speed
#define ADC_usDELAY 5000L
#define DELAY_US(A)  DSP28x_usDelay(((((long double) A * 1000.0L) / (long double)CPU_CLOCK_SPEED) - 9.0L) / 5.0L)

extern void DSP28x_usDelay(unsigned long Count);

void F281X_adc04u_drv_init(void)
{
    DELAY_US(ADC_usDELAY);	
  
    AdcRegs.ADCTRL1.all = ADC_RESET_FLAG; 		// Reset the ADC Module   
	asm(" NOP ");
	asm(" NOP ");    

    AdcRegs.ADCTRL3.bit.ADCBGRFDN = 0x3;		// Power up bandgap/reference circuitry 
	DELAY_US(ADC_usDELAY);			    		// Delay before powering up rest of ADC 
    
    AdcRegs.ADCTRL3.bit.ADCPWDN = 1;	   		// Power up rest of ADC
    AdcRegs.ADCTRL3.bit.ADCCLKPS = 6;     		// Set up ADCTRL3 register
//    AdcRegs.ADCTRL3.bit.ADCCLKPS = 3; //
	DELAY_US(ADC_usDELAY);	

    AdcRegs.ADCTRL1.all = ADC_SUS_MODE0 + ADC_ACQ_PS_2 + ADC_CPS_1 + ADC_SEQ_CASC;	//
	
    AdcRegs.ADCTRL2.all = ADC_EVA_SOC_SEQ1; 	// Set up ADCTRL2 register 
	AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1 = 1;
	AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;

	AdcRegs.ADCMAXCONV.bit.MAX_CONV1 = 3;               // Specify four conversions 
    AdcRegs.ADCCHSELSEQ1.all = 0x3232;      	    // Configure channel selection 

	EvaRegs.GPTCONA.bit.T2TOADC = 1;      		        // 设置定时器T2触发AD转换
}  
////////////////////////////////////////////////////////////////////////////

/*
void F281X_EV1_BLDC_PWM_Init(void) 
{       
        EvaRegs.T1PR = p->PeriodMax;             // Init Timer 1 Period Register
        EvaRegs.T1CON.all = FREE_RUN_FLAG + TIMER_CONT_UP + TIMER_CLK_PRESCALE_X_1 + TIMER_ENABLE;   // Init PWM Operation
//        EvaRegs.ACTRA.all = 0x0000;                
//        EvaRegs.GPTCONA.all = 0x0000;
//        EvaRegs.COMCONA.all = 0xA200;

//        EALLOW;                       // Enable EALLOW
//        GpioMuxRegs.GPAMUX.all |= 0x003F;    // Setting PWM1-6 as primary output pins
//        EDIS;                         // Disable EALLOW
}
*/
void main(void)
{

// ******************************************
// Initialization code for DSP_TARGET = F2812
// ******************************************
// Initialize System Control registers, PLL, WatchDog, Clocks to default state:
        // This function is found in the DSP281x_SysCtrl.c file.
	InitSysCtrl();

// HISPCP prescale register settings, normally it will be set to default values
    EALLOW;   // This is needed to write to EALLOW protected registers
    SysCtrlRegs.HISPCP.all = 0x0000;     // SYSCLKOUT/1 
    EDIS;   // This is needed to disable write to EALLOW protected registers 

// Disable and clear all CPU interrupts:
	DINT;
	IER = 0x0000;
	IFR = 0x0000;

// Initialize Pie Control Registers To Default State:
        // This function is found in the DSP281x_PieCtrl.c file.
	InitPieCtrl();

// Initialize the PIE Vector Table To a Known State:
        // This function is found in DSP281x_PieVect.c.
	// This function populates the PIE vector table with pointers
        // to the shell ISR functions found in DSP281x_DefaultIsr.c.
	InitPieVectTable();	
	
// User specific functions, Reassign vectors (optional), Enable Interrupts:
	
// Initialize EVA Timer 1/2:
    // Setup Timer 1/2 Registers (EV A)
    EvaRegs.GPTCONA.all = 0;

    // Set the Period for the GP timer 2
    EvaRegs.T2PR = 150000/20;  // ISR_FREQUENCY = 20kHz
    
    // Clear the counter/compare Regs for GP timer 2
    EvaRegs.T2CNT = 0x0000;
//    EvaRegs.T2CMPR = 0x0000;  

	nLastStatus=nNowStatus=0;

// Enable Period interrupt bits for GP timer 2
//    EvaRegs.EVAIMRB.bit.T2PINT = 1;
//    EvaRegs.EVAIFRB.bit.T2PINT = 1;

    // Count up, x1, internal clk, disable compare, use own period
    EvaRegs.T2CON.all = 0x9040;//连续增计数，预标定为HSPCLK，使能定时器，内部时钟，计数值为0时重装载，禁止比较操作

// Reassign ISRs. 
        // Reassign the PIE vector for T2PINT to point to a different 
        // ISR then the shell routine found in DSP281x_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.T2PINT = &MainISR;
	PieVectTable.ADCINT = &MainISR;
	EDIS;   // This is needed to disable write to EALLOW protected registers

// Enable PIE group 3 interrupt 1 for T2PINT
//    PieCtrlRegs.PIEIER3.all = M_INT1;
    PieCtrlRegs.PIEIER1.bit.INTx6 = 1;
// Enable CPU INT3 for T2PINT:
//	IER |= M_INT3;
	IER |= M_INT1;


       EALLOW;                       // Enable EALLOW
       GpioMuxRegs.GPAMUX.all &= 0xFFC0;    // PWM1-6输出方式为I/O
       GpioMuxRegs.GPADIR.all |= 0x003F;    // PWM1-6设置为输出，而非输入
       EDIS;                         // Disable EALLOW 

	F281X_adc04u_drv_init();

// Initialize the SPEED_PR module (150 MHz, N = 1 event period/rev)
 	speed1.InputSelect = 0;
 	speed1.BaseRpm = 60*(BASE_FREQ/P)/6;	//原来为120，这里P为极对数而非极数，故改为60
											//速度值改为1/6周更新一次，故需除6
 	speed1.SpeedScaler = (Uint32)(20/(1*BASE_FREQ*0.001));//ISR_FREQUENCY = 20kHz
///////////////////////////////////////////////

/*
// Initialize RMPCNTL module

    rc1.RampDelayMax = 20;
    rc1.RampLowLimit = _IQ(0);
    rc1.RampHighLimit = _IQ(1);
*/

// Initialize Hall module   
    hall1.DebounceAmount = 5;
    hall1.Revolutions = -10;
    hall1.init(&hall1);
/*
// Initialize RMP2 module
	rmp2.Out = (int32)DFuncDesired;
	rmp2.Ramp2Delay = 0x00000050;
    rmp2.Ramp2Max = 0x00007FFF;
    rmp2.Ramp2Min = 0x0000000F;

// Initialize the PID_REG3 module for dc-bus current
    pid1_idc.Kp = _IQ(1);                  
	pid1_idc.Ki = _IQ(T/0.003);			
	pid1_idc.Kd = _IQ(0/T);					
 	pid1_idc.Kc = _IQ(0.2);
    pid1_idc.OutMax = _IQ(0.99);
    pid1_idc.OutMin = _IQ(0); 

// Initialize the PID_REG3 module for speed
    pid1_spd.Kp = _IQ(1);
	pid1_spd.Ki = _IQ(T/0.1);
	pid1_spd.Kd = _IQ(0/T);
 	pid1_spd.Kc = _IQ(0.2);
    pid1_spd.OutMax = _IQ(0.99);
    pid1_spd.OutMin = _IQ(0); 

// Initialize the ICETEK_PIDA module for speed
    pid2_spd.Kp = _IQ(0.06);
	pid2_spd.Ki = _IQ(0.02);
	pid2_spd.Kd = _IQ(0.01);
    pid2_spd.OutMax = _IQ(0.001);
    pid2_spd.OutMin = _IQ(-0.001); 
*/
// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

// IDLE loop. Just sit and loop forever:	
	for(;;)
	{
		BackTicker++;
		//if ( nADCValue<20 )	EnableFlag=TRUE;
	}
} 	

interrupt void MainISR(void)
{
//DINT;
// Verifying the ISR
     IsrTicker++;

/////////////////////////////////////////////////////////////////
	    while (AdcRegs.ADCST.bit.SEQ1_BSY == 1)
        {};

        vadc1 = (AdcRegs.ADCRESULT0>>4)&0x0FFF;
        vadc2 = (AdcRegs.ADCRESULT1>>4)&0x0FFF;
        vadc3 = (AdcRegs.ADCRESULT2>>4)&0x0FFF;
        vadc4 = (AdcRegs.ADCRESULT3>>4)&0x0FFF;
//        AdcRegs.ADCTRL2.all |= 0x4040;       	    // Reset the sequence
		vadc1 = (vadc1 + vadc3 )/2;
		vadc2 = (vadc2 + vadc4 )/2;

		vadc1_1=(vadc1 + vadc1_1)/2;
		vadc2_1=(vadc2 + vadc2_1)/2;

		iadc1 = -(vadc1 - 2160) /102.375/1.557;
		iadc2 = -(vadc2 - 2128) /102.375/1.545;

		//if ( EnableFlag==FALSE )	goto mmexit;//只有在确认AD正常工作后再启动电机控制

		if (IsrTicker % 4 == 0)     goto sample_exit;

		if(iadc1 > 2 || iadc1 < -2 || iadc2 > 2 || vadc2 < -2 || empc_flag)//相电流过流保护
			{
        		GpioDataRegs.GPACLEAR.all |= 0x003F;    //关闭所有PWM
				goto sample_exit;
			}


//float X0[3];
//Uint16 U_pwm;
		//EMPC计算部分
		X0[0] = iadc1;
		X0[1] = iadc2;
		X0[2] = _IQ27toF(the_r);
	//	U_pwm = mpt_getInput(X0);
U_pwm = mpt_searchTree(X0); //U_pwm 
	//	U_pwm = U_pwm & 0x003F;

sample_exit:
// ------------------------------------------------------------------------------
//    Connect inputs of the HALL module and call the Hall sensor
//    read function.
// ------------------------------------------------------------------------------
      hall1.HallMapPointer = (int16)mod1.Counter; 
      hall1.read(&hall1);
// ------------------------------------------------------------------------------
//    Connect inputs of the MOD6 module and call the Modulo 6 counter
//    calculation function.
// ------------------------------------------------------------------------------
      mod1.TrigInput =(int32)hall1.CmtnTrigHall;
      mod1.Counter = (int32)hall1.HallMapPointer; 
      mod1.calc(&mod1);

// ------------------------------------------------------------------------------
//    Set the speed closed loop flag once the speed is built up to a desired value. 
// ------------------------------------------------------------------------------
	  nISRWork1=(int16)hall1.HallGpio;
	  bISRWork=( nISRWork1>0 && nISRWork1<7 )?(1):(0);
	  if ( bISRWork )	
	  {
	    nISRWork1--;
		nLastStatus=nNowStatus;
	  	nNowStatus=nHallABC[nISRWork1];
	  }


	  if ( bISRWork&&nNowStatus!=nLastStatus)//&&nNowStatus==0 每1/6周更新一次速度
	  {
         speed1.TimeStamp = VirtualTimer;
         speed1.calc(speed1);
		 nSpeed=speed1.SpeedRpm;
speed_count=speed1.EventPeriod;
//int32 speed_count;
		 the_r_base = _IQ27mpy(_IQ27(nNowStatus) , _IQ27(PI/3)) + _IQ27(PI/6);//由hall推算得到的电角度
		 d_the_rp = _IQ27mpy(_IQ27(nSpeed) , _IQ27(2 * PI  / 60 / 20000));//每次采样增加的电角度估计值
		 counter_the_r = 0;//电角度计数器
		 
		 //bang-bang 速度环
		// SpeedRef =1000;
		empc_flag = nSpeed-900 > 1? 1:0;

	  }

	  counter_the_r++ ;
	  d_the_r = _IQ27mpy(_IQ27(counter_the_r) , _IQ27(d_the_rp));//离上次hall采样转过总电角度估计值
	  if(d_the_r > _IQ27(1.047))//1.047<PI/3，避免换向间隙的影响
		d_the_r = _IQ27(1.047);
	  the_r = d_the_r + the_r_base;
	  if(the_r > _IQ27(2 * PI) )
		the_r = the_r - _IQ27(2 * PI);


// ------------------------------------------------------------------------------
//    Increase virtual timer and force 15 bit wrap around
// ------------------------------------------------------------------------------
	VirtualTimer++;
	VirtualTimer &= 0x00007FFF;

       // GpioDataRegs.GPADAT.all |= 0x003F;    // Setting PWM1-6 as primary output pins
	iadc3=-iadc1-iadc2;
switch(nNowStatus)
{
	case 0: te=0.018*(iadc1-iadc2);
			wiz= iadc3; break;
	case 1: te=0.018*(iadc1-iadc3);
			wiz= iadc2; break;
	case 2: te=0.018*(iadc2-iadc3);
			wiz= iadc1; break;
	case 3: te=0.018*(iadc2-iadc1);
			wiz= iadc3; break;
	case 4: te=0.018*(iadc3-iadc1);
			wiz= iadc2; break;
	case 5: te=0.018*(iadc3-iadc2);
			wiz= iadc1; break;
	default: te=0.09;
}

//float iadc3,te,wiz
          
mmexit:

	AdcRegs.ADCTRL2.bit.RST_SEQ1=1;//复位排序器
	AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;	//清排序器1中断标志
	PieCtrlRegs.PIEACK.all |= PIEACK_GROUP1;//清PIE1组中断标志
//	EINT;	
}

//===========================================================================
// No more.
//===========================================================================