pmsm(一个网友的28335电机程序).txt

tms320f28335产生pwm信号的程序

#include "DSP2833x_Device.h"     // DSP2833x Headerfile Include File
#include "DSP2833x_Examples.h"   // DSP2833x Examples Include File
#include "math.h"

#include "pmsm.h"

#define PI          3.1415926
#define adc0_offset 1960        
#define adc1_offset 1960
#define Sample_PRD  0.0001      //0.1ms
#define Giv_Frq     10          //sine wave frequency
#define Udc         160
#define Giv_Vol     (0.1 * Udc / 2)
#define sqrt3       1.7320508
#define sqrt2       1.4142136
//#define PWM_Prd     15000       //15000*6.67ns(5K) 
#define Encoder_r   (2500*4)
#define Polepairs   4
#define LED	*((Uint16 *)0x4080)

unsigned int ADC_0, ADC_1;
unsigned long QepCount,QepCount_last=0,QepCount_last_1=0,QepLatch;
long Theta_m,Theta_m_last=0, Theta_e_ul;
double Theta_e, e_angle=0.0,Ramp=0.0,Speed=0.0;
double Ialpha, Ibeta, Id, Iq, Ud,Uq;
double Ualpha,Ubeta,Ualpha_test,Ubeta_test;

double sInteger=0.0, sDerivation=0.0, sDevOutput_last=0.0;
double dInteger=0.0, dDerivation=0.0, dDevOutput_last=0.0;
double qInteger=0.0, qDerivation=0.0, qDevOutput_last=0.0;

double Ia_x,Ib_x,Ia_x_last1=0.0, Ib_x_last1 =0.0,Ia_x_last2=0.0, Ib_x_last2 =0.0;
double Ia_y,Ib_y,Ia_y_last1=0.0,Ib_y_last1=0.0,Ia_y_last2=0.0,Ib_y_last2=0.0;

double Idref = 0, kpd =0.15, kid =0.0015, kicd =0.01, kdd =0, maxd = 20.0, mind = -20.0; 
double Iqref = 0, kpq =0.15, kiq =0.0015, kicq =0.01, kdq =0, maxq = 80.0, minq = -80.0; 
double Spdref = 10, kps =1.20, kis =0.001, kics =0.005,kds =0, maxs = 8.0, mins = -8.0; 

void OneStep()
{
  int sector;
  double Va,Vb,Vc;
  double Ta,Tb,Tc;

  // Read from ADC
  ADC_0 = (AdcRegs.ADCRESULT8) >> 4;
  ADC_1 = (AdcRegs.ADCRESULT9) >> 4;
  AdcRegs.ADCTRL2.bit.RST_SEQ2 = 0x1;// Sequencer SEQ2 reset

  Ia_x = (double)((double)ADC_0 - (double)adc0_offset)/2048.0*10.0;
  if(Ia_x>10.0) Ia_x = 10.0; 
  else if(Ia_x<-10.0) Ia_x = -10.0;
  Ib_x = (double)((double)ADC_1 - (double)adc1_offset)/2048.0*10.0;
  if(Ib_x>10.0) Ib_x = 10.0; 
  else if(Ib_x<-10.0) Ib_x = -10.0;
  
  //Qep 采集
  QepCount = EQep2Regs.QPOSCNT;//eQEP Position Counter
  QepLatch = EQep2Regs.QPOSILAT;// The position-counter value is latched into this register on an IEL

/*  ，时间为1S  */
  if(initcount++ <= 15000)  //初始定位过程
  {
     LED = 1;
//     Iqref = 2.0;       //采用给定（Iq，Theta)=（2,1.5PI）方式启动
     Ud = 10.0;           //采用给定（Ud, Uq, Theta)=（10, 2, 1.5PI）方式启动
     Uq = 2.0;
     Theta_e = 1.5 * PI;
     EQep2Regs.QPOSCNT = 0;
  }
  else    //正常运行过程
  {
    LED = 0;
    initcount = 20000;

    if(++loopcount > 39)
    {
      loopcount = 0;

      //计算电机角速度
      if(EQep2Regs.QEPSTS.bit.QDF == 1)    //clockwise
      {
         if(QepCount < QepCount_last)
            Speed = (65536.0+(double)QepCount - (double)QepCount_last)* 250.0 * 60.0 / (double)Encoder_r;
         else
            Speed = ((double)QepCount - (double)QepCount_last)* 250.0 * 60.0 / (double)Encoder_r;
      } 
      else
      {
         if(QepCount > QepCount_last)
            Speed = (-65536.0+(double)QepCount - (double)QepCount_last)* 250.0 * 60.0 / (double)Encoder_r;
         else
            Speed = ((double)QepCount - (double)QepCount_last)* 250.0 * 60.0 / (double)Encoder_r;
      } 
      QepCount_last = QepCount;

      if(Spdref < 300)  //给定速度
         Spdref = Spdref + 1.0;
	  //速度环PID
      {
	double err, Output, Gain, Saterr;
	err = Spdref - Speed;
	Gain = kps * err;
	Output = Gain + sInteger + sDerivation;
	if(Output > maxs)
	{
	    Iqref = maxs;
	}
	else if(Output < mins)
	{
	   Iqref = mins;
	}
	else
	{
	  Iqref = Output;
	}
	Saterr = Iqref - Output;
	sInteger = sInteger + kis * Gain + kics * Saterr;
	sDerivation = kds * (Gain - sDevOutput_last);
	sDevOutput_last = Gain;
      }
    }

    //计算机械角度 
    if(EQep2Regs.QEPSTS.bit.QDF == 1)    //clockwise
    {	
       if(QepCount < QepCount_last_1)
         Theta_m = QepCount - QepCount_last_1 + 65536 + Theta_m_last;
       else
       {
	  Theta_m = QepCount - QepCount_last_1 + Theta_m_last;
       }
    } 
    else                              //counter-clockwise  >>>>>>>>
    {
       if(QepCount_last_1 < QepCount)
 	Theta_m = QepCount - QepCount_last_1 - 65536 + Theta_m_last;
       else
       {
         Theta_m = QepCount - QepCount_last_1 + Theta_m_last;
       }
    }
    QepCount_last_1 = QepCount;
    Theta_m_last = Theta_m;
    if(Theta_m >= Encoder_r)
        Theta_m = Theta_m - Encoder_r;
    else if(Theta_m <= -Encoder_r)
        Theta_m = Theta_m + Encoder_r;

     //计算电角度
     Theta_e_ul =(Theta_m - Theta_m / (Encoder_r / Polepairs) * (Encoder_r / Polepairs));
     if(Theta_e_ul < 0)
            Theta_e_ul = -Theta_e_ul;
     Theta_e = (double)Theta_e_ul * 2 * PI / (double)(Encoder_r / Polepairs);

     //Clark 变换
     Ialpha = Ia_x;
     Ibeta = (Ia_x + 2 * Ib_x) / sqrt3;

     //Park 变换
     Id = Ialpha * cos(Theta_e) + Ibeta * sin(Theta_e);
     Iq = -Ialpha * sin(Theta_e) + Ibeta * cos(Theta_e);

     //PID for Id;
     {
	  double err, Output, Gain, Saterr;
	  err = Idref - Id;
	  Gain = kpd * err;
	  Output = Gain + dInteger + dDerivation;
	  if(Output > maxd)
	  {
	    Ud = maxd;
	  }
	  else if(Output < mind)
	  {
	    Ud = mind;
	  }
	  else
	  {
	    Ud = Output;
	  }
	  Saterr = Ud - Output;
	  dInteger = dInteger + kid * Gain + kicd * Saterr;
	  dDerivation = kdd * (Gain - dDevOutput_last);
	  dDevOutput_last = Gain;
     }
     //PID for Iq;
     {
	  double err, Output, Gain, Saterr;
	  err = Iqref - Iq;
	  Gain = kpq * err;
	  Output = Gain + qInteger + qDerivation;
	  if(Output > maxq)
	  {
	    Uq = maxq;
	  }
	  else if(Output < minq)
	  {
	    Uq = minq;
	  }
	  else
	  {
	    Uq = Output;
	  }
	  Saterr = Uq - Output;
	  qInteger = qInteger + kiq * Gain + kicq * Saterr;
	  qDerivation = kdq * (Gain - dDevOutput_last);
	  qDevOutput_last = Gain;
     }
  }  //if(>15000)
	  
/*    // Park逆变换	*/  
    Ualpha = Ud * cos(Theta_e) - Uq * sin(Theta_e);
    Ubeta = Ud * sin(Theta_e) + Uq * cos(Theta_e);

/*  //电角度生成0-2*PI,	  这段程序为开环时用，包括生成电角度和计算Ualpha，Ubeta
  {
     if(Ramp <= Giv_Frq)
	  Ramp += 0.002;   
     e_angle += Ramp * Sample_PRD * (2*PI);
     if(e_angle >= 2*PI)
	  e_angle = 0.0;
     Ualpha = Giv_Vol * sin(e_angle);//
     Ubeta = Giv_Vol * cos(e_angle);
  }		
*/
  //生成SVPWM
  sector = 0;
  Va = Ubeta;
  Vb = -0.5 * Ubeta + 0.5 * sqrt3 * Ualpha;
  Vc = -0.5 * Ubeta - 0.5 * sqrt3 * Ualpha;

  if(Va >0)
    sector = 1;
  if(Vb>0)
    sector = sector + 2;
  if(Vc>0)
    sector = sector + 4;

  Vb = 0.5 * Ubeta + 0.5 * sqrt3 * Ualpha;
  Vc = 0.5 * Ubeta - 0.5 * sqrt3 * Ualpha;
  if(sector == 0)
  {
    Ta = 0.5;
    Tb = 0.5;
    Tc = 0.5;
  }
  else if(sector == 1)
  {
    Tb = 0.5 * (Udc - Vc - Vb);
    Ta = Tb + Vc;
    Tc = Ta + Vb;
  }
  else if(sector == 2)
  {
    Ta = 0.5 * (Udc -Vb + Va);
    Tc = Ta + Vb;
    Tb = Tc - Va;
  }
  else if(sector == 3)
  {
    Ta = 0.5 * (Udc + Vc - Va);
    Tb = Ta - Vc;
    Tc = Tb + Va;
  }
  else if(sector == 4)
  {
    Tc = 0.5 * (Udc + Va - Vc);
    Tb = Tc - Va;
    Ta = Tb + Vc;
  }
  else if(sector == 5)
  {
    Tb = 0.5 * (Udc - Va + Vb);
    Tc = Tb + Va;
    Ta = Tc - Vb;
  }
  else if(sector == 6)
  {
    Tc = 0.5 * (Udc + Vb + Vc);
    Ta = Tc - Vb;
    Tb = Ta - Vc;
  }
  Ta = 2 * (Ta / Udc - 0.5);
  Tb = 2 * (Tb / Udc - 0.5);
  Tc = 2 * (Tc / Udc - 0.5);

  EPwm4Regs.CMPA.half.CMPA = (unsigned int)((Ta + 0.5)* PWM_Prd);
  EPwm5Regs.CMPA.half.CMPA = (unsigned int)((Tb + 0.5)* PWM_Prd);
  EPwm6Regs.CMPA.half.CMPA = (unsigned int)((Tc + 0.5)* PWM_Prd);
}

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