pmsm3_4.c

移植TI的永磁同步电机（PMSM）速度闭环调速程序

/* ==============================================================================
System Name:  PMSM34

File Name:	PMSM3_4.C

Description:	Primary system file for the Real Implementation of Position Control
                Based Sensored Field Orientation Control for a Three Phase Permanent-
                Magnet Synchronous Motor (PMSM) using QEP sensor

Originator:		Digital control systems Group - Texas Instruments

Note: In this software, the default inverter is supposed to be DMC550 board. 
=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20: Support both F280x and F281x targets 
 04-25-2005 Version 3.21: Move EINT and ERTM down to ensure that all initialization
 						  is completed before interrupts are allowed.
=================================================================================  */

#include "typedef.h"

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

#if (DSP_TARGET==F2808)
#include "DSP280x_Device.h"
#endif
#if (DSP_TARGET==F2812)
//#include "DSP281x_Device.h"
#include "DSP28_Device.h"
#endif

#include <stdio.h>
#include <stdlib.h>

#include "IQmathLib.h"
#include "pmsm3_4.h"
#include "parameter.h"
#include "build.h"
#include "math.h"

#include "beep.h"
#include "usbdef.h"


// Prototype statements for functions found within this file.
interrupt void PmsmISR(void);
#if (DSP_TARGET==F2812)
interrupt void QepISR(void);
#endif

// Global variables used in this system
float32 VdTesting = 0;            // Vd testing (pu) 
float32 VqTesting = 0.25;         // Vq testing (pu) 
float32 IdRef = 0;                // Id reference (pu) 
float32 IqRef = 0.2;              // Iq reference (pu) 
float32 SpeedRef = 0.1;           // Speed reference (pu) 
float32 PositionRef = 0.5;      // Position reference (Mechanical rotor Anglele  (pu) 
float32 T = 0.001/ISR_FREQUENCY;  // Samping period (sec), see parameter.h 

Uint16 IsrTicker = 0;
Uint16 BackTicker = 0;

int16 PwmDacCh1=0;
int16 PwmDacCh2=0;
int16 PwmDacCh3=0;

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

volatile Uint16 EnableFlag = FALSE;
Uint16 LockRotorFlag = FALSE;

Uint16 SpeedLoopPrescaler = 10;//10;      // Speed loop prescaler
Uint16 SpeedLoopCount = 1;           // Speed loop counter

// Instance a few transform objects
CLARKE clarke1 = CLARKE_DEFAULTS;
PARK park1 = PARK_DEFAULTS;
IPARK ipark1 = IPARK_DEFAULTS;

// Instance PID regulators to regulate the d and q synchronous axis currents,
// speed and position
PIDREG3 pid1_id  = PIDREG3_DEFAULTS;
PIDREG3 pid1_iq  = PIDREG3_DEFAULTS;
PIDREG3 pid1_pos = PIDREG3_DEFAULTS;
PIDREG3 pid1_spd = PIDREG3_DEFAULTS;

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

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

// Instance a Space Vector PWM modulator. This modulator generates a, b and c
// phases based on the d and q stationery reference frame inputs
SVGENDQ svgen_dq1 = SVGENDQ_DEFAULTS;

// Instance a QEP interface driver 
QEP qep1 = QEP_DEFAULTS;

// Instance a speed calculator based on QEP
SPEED_MEAS_QEP speed1 = SPEED_MEAS_QEP_DEFAULTS;

// Instance a enable PWM drive driver (only for DMC1500) 
DRIVE drv1 = DRIVE_DEFAULTS;

// Instance a ramp controller to smoothly ramp the frequency
RMPCNTL rc1 = RMPCNTL_DEFAULTS;

//	Instance a ramp generator to simulate an Anglele
RAMPGEN rg1 = RAMPGEN_DEFAULTS;

// Create an instance of the current/dc-bus voltage measurement driver
PHASEVOLTAGE volt1 = PHASEVOLTAGE_DEFAULTS;
ILEG2DCBUSMEAS ilg2_vdc1 = ILEG2DCBUSMEAS_DEFAULTS;

// Create an instance of DATALOG Module
DLOG_4CH dlog = DLOG_4CH_DEFAULTS;  

// 外部监控变量接口
#include "usbdef.h"

extern unsigned gausgafOscUserVarSelected[OSC_USER_VAR_MAXCOUNT + 1];
extern short    gafOscUserVarData[OSC_USER_VAR_MAXCOUNT + 1];
extern short    asOscSamData[16];

    // (0, "Theta");  // 设定用户通道定义
    // (1, "PWM_Tu");
    // (2, "PWM_Tv");
    // (3, "PWM_Tw");
    // (4, "Vds");
    // (5, "Vqs");
    // (6, "Flux_ds");
    // (7, "Flux_qs");
    // (8, "Vdr");
    // (9, "Vqr");
    // (10, "Flux_dr");
    // (11, "Flux_qr");
    // (12, "Rpm_M");
    // (13, "Rpm_E");
    //
    
#define MONITOR_THETA(Theta)        {if(gausgafOscUserVarSelected[0])  gafOscUserVarData[0]  = Theta;}
#define MONITOR_PWMTU(PWM_Tu)       {if(gausgafOscUserVarSelected[1]) gafOscUserVarData[1] = PWM_Tu;}
#define MONITOR_PWMTV(PWM_Tv)       {if(gausgafOscUserVarSelected[2]) gafOscUserVarData[2] = PWM_Tv;}
#define MONITOR_PWMTW(PWM_Tw)       {if(gausgafOscUserVarSelected[3]) gafOscUserVarData[3] = PWM_Tw;}
#define MONITOR_VDS(Vds)            {if(gausgafOscUserVarSelected[4])  gafOscUserVarData[4]  = Vds;}
#define MONITOR_VQS(Vqs)            {if(gausgafOscUserVarSelected[5])  gafOscUserVarData[5]  = Vqs;}
#define MONITOR_FLUX_DS(Flux_ds)    {if(gausgafOscUserVarSelected[6])  gafOscUserVarData[6]  = Flux_ds;}
#define MONITOR_FLUX_QS(Flux_qs)    {if(gausgafOscUserVarSelected[7])  gafOscUserVarData[7]  = Flux_qs;}
#define MONITOR_VDR(Vdr)            {if(gausgafOscUserVarSelected[8])  gafOscUserVarData[8]  = Vdr;}
#define MONITOR_VQR(Vqr)            {if(gausgafOscUserVarSelected[9]) gafOscUserVarData[9] = Vqr;}
#define MONITOR_FLUX_DR(Flux_dr)    {if(gausgafOscUserVarSelected[10]) gafOscUserVarData[10] = Flux_dr;}
#define MONITOR_FLUX_QR(Flux_qs)    {if(gausgafOscUserVarSelected[11]) gafOscUserVarData[11] = Flux_qs;}
#define MONITOR_RPM_M(Rpm_M)        {if(gausgafOscUserVarSelected[12]) gafOscUserVarData[12] = Rpm_M;}
#define MONITOR_RPM_E(Rpm_E)        {if(gausgafOscUserVarSelected[13]) gafOscUserVarData[13] = Rpm_E;}

float fTheta, fPWMTu, fPWMTv, fPWMTw;
float fVa, fVb, fVc, fIa, fIb, fIc;
float fRpmM, fRpmE;                       //  测量转速, 估计转速
float fVds, fVqs, fFluxDs, fFluxQs;     //   定子DQ 轴电压和磁链
float fVdr, fVqr, fFluxDr, fFluxQr;     //   转子DQ 轴电压和磁链

float fMotorFreq;
long lMotorRpmMeas, lMotorRpmEst;      //  电机转速实际测量值和估计值

#define PMSM_OPEN_LOOP   1
#define PMSM_CLOSE_LOOP  2
BOOL    bMode;  
unsigned short ucIndexCount = 0;

void F2812_EV1_PWM_Init(PWMGEN *p) 
{       
        EALLOW;                       /* Enable EALLOW */
        EvaRegs.T1PR = p->PeriodMax;             /* Init Timer 1 period Register  */
        EvaRegs.T1CON.all   = PWM_INIT_STATE;     /* Symmetrical Operation         */
        EvaRegs.DBTCONA.all = DBTCON_INIT_STATE;                                    
        EvaRegs.ACTRA.all   = 0x0FFF;             // 关闭PWM输出, 强制为高
        //EvaRegs.ACTRA.all = ACTR_INIT_STATE;                

        EvaRegs.COMCONA.all = 0xA200;

        EvaRegs.CMPR1 = p->PeriodMax;
        EvaRegs.CMPR2 = p->PeriodMax;
        EvaRegs.CMPR3 = p->PeriodMax;
        
        GpioMuxRegs.GPAMUX.all |= 0x003F; 
        EDIS;                         /* Disable EALLOW */
}

void PMSM_Init(void)
{

// ******************************************
// Initialization code for DSP_TARGET = F2808
// ******************************************
#if (DSP_TARGET==F2808)

// Initialize System Control registers, PLL, WatchDog, Clocks to default state:
        // This function is found in the DSP280x_SysCtrl.c file.
	InitSysCtrl();

// Globally synchronize all ePWM modules to the time base clock (TBCLK)
    EALLOW;
    SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1;
    EDIS;

// 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 DSP280x_PieCtrl.c file.
	InitPieCtrl();

// Initialize the PIE Vector Table To a Known State:
        // This function is found in DSP280x_PieVect.c.
	// This function populates the PIE vector table with pointers
        // to the shell ISR functions found in DSP280x_DefaultIsr.c.
	InitPieVectTable();	
	
// User specific functions, Reassign vectors (optional), Enable Interrupts:

   // Waiting for enable flag set
   while (EnableFlag==FALSE) 
    { 
      BackTicker++;
    }
    
// Enable CNT_zero interrupt using EPWM1 Time-base
    EPwm1Regs.ETSEL.bit.INTEN = 1;   // Enable EPWM1INT generation 
    EPwm1Regs.ETSEL.bit.INTSEL = 1;  // Enable interrupt CNT_zero event
    EPwm1Regs.ETPS.bit.INTPRD = 1;   // Generate interrupt on the 1st event
	EPwm1Regs.ETCLR.bit.INT = 1;     // Enable more interrupts

// Reassign ISRs. 
        // Reassign the PIE vector for EPWM1_INT 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.EPWM1_INT = &MainISR;
	EDIS;   // This is needed to disable write to EALLOW protected registers

// Enable PIE group 3 interrupt 1 for EPWM1_INT
    PieCtrl.PIEIER3.all = M_INT1;

// Enable CPU INT3 for EPWM1_INT:
	IER |= M_INT3;

#endif


// ******************************************
// Initialization code for DSP_TARGET = F2812
// ******************************************
#if (DSP_TARGET==F2812)

    DINT;

    #if 0   // 主程序已经初始化

    // 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:
    #endif	
    
// Initialize EVA Timer 1:
    // Setup Timer 1 Registers (EV A)
    EvaRegs.GPTCONA.all = 0;

    #if 0   // 转移到电机启动函数中
       // Waiting for enable flag set
       while (EnableFlag==FALSE) 
        { 
          BackTicker++;
        }

    // Enable Underflow interrupt bits for GP timer 1
        EvaRegs.EVAIMRA.bit.T1UFINT = 1;
        EvaRegs.EVAIFRA.bit.T1UFINT = 1;

    // Enable CAP3 interrupt bits for GP timer 2
        EvaRegs.EVAIMRC.bit.CAP3INT = 1;
        EvaRegs.EVAIFRC.bit.CAP3INT = 1;
    #endif
// Reassign ISRs. 
        // Reassign the PIE vector for T1UFINT and CAP3INT 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.T1UFINT = &PmsmISR;
	PieVectTable.CAPINT3 = &QepISR;
	EDIS;   // This is needed to disable write to EALLOW protected registers
	
    #if 0   // 转移到电机启动函数中
    // Enable PIE group 2 interrupt 6 for T1UFINT
        PieCtrl.PIEIER2.all = M_INT6;

    // Enable PIE group 3 interrupt 7 for CAP3INT
        PieCtrl.PIEIER3.all = M_INT7;

    // Enable CPU INT2 for T1UFINT and INT3 for CAP3INT:
    	IER |= (M_INT2 | M_INT3);
    #endif
#endif

// Initialize PWM module
    pwm1.PeriodMax = SYSTEM_FREQUENCY*1000000*T/2;  // Perscaler X1 (T1), ISR period = T x 1 
    F2812_EV1_PWM_Init(&pwm1);
	//pwm1.init(&pwm1); 

// Initialize PWMDAC module
    pwmdac1.PeriodMax = (SYSTEM_FREQUENCY*200/(30*2))*5;   // PWMDAC Frequency = 30 kHz
    pwmdac1.PwmDacInPointer0 = &PwmDacCh1;
    pwmdac1.PwmDacInPointer1 = &PwmDacCh2;
    pwmdac1.PwmDacInPointer2 = &PwmDacCh3;
	pwmdac1.init(&pwmdac1); 

// Initialize DATALOG module
    dlog.iptr1 = &DlogCh1;
    dlog.iptr2 = &DlogCh2;
    dlog.iptr3 = &DlogCh3;
    dlog.iptr4 = &DlogCh4;
    dlog.trig_value = 0x1;
    dlog.size = 0x400;
    dlog.prescalar = 1;
    dlog.init(&dlog);

// Initialize QEP module
    qep1.LineEncoder = 2500;
    qep1.MechScaler = _IQ30(0.25/qep1.LineEncoder);
    qep1.PolePairs = P/2;
    qep1.CalibratedAngle = 875;//-1650;//-375;//-1250;
    qep1.init(&qep1);

// Initialize the Speed module for QEP based speed calculation
    speed1.K1 = _IQ21(1/(BASE_FREQ*T));
    speed1.K2 = _IQ(1/(1+T*2*PI*30));  // Low-pass cut-off frequency