pmsm3_4.c

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

    speed1.K3 = _IQ(1)-speed1.K2;
    speed1.BaseRpm = 120*(BASE_FREQ/P);

// Initialize enable drive module (FOR DMC1500 ONLY)   
    drv1.init(&drv1);

    #if 0   // 去掉该项，因为会修改用户的ADC初始化值
    // Initialize ADC module
    // Note for DMC550: 
    //  - At 24 dc-bus volt, the ADC input for measured Vdc_bus range is 24*1/(24.9+1) = 0.927 volt
    //  - Then, Vdc_bus gain = 3.0/0.927 = 3.2375 (or 0x675C in Q13) 
        ilg2_vdc1.VdcMeasGain = 0x675C;
        ilg2_vdc1.ChSelect = 0x0610;
        ilg2_vdc1.init(&ilg2_vdc1);
    #endif

// Initialize the RAMPGEN module
    rg1.StepAngleMax = _IQ(T);//_IQ(BASE_FREQ*T);
    //rg1.Gain         = _IQ(P/2);

// Initialize the RAMPGEN module
    rc1.RampDelayMax = 1;

// Initialize the PID_REG3 module for Id
	pid1_id.Kp = _IQ(0.01);    //_IQ(0.1);
	pid1_id.Ki = _IQ(T/0.02);//_IQ(T/0.02);			
	pid1_id.Kd = _IQ(0/T); 						
	pid1_id.Kc = _IQ(0);//_IQ(0.5);

	/*  ----------------------    重要关注    ---------------------------
	
	    !!!  Id的PID输出为标幺值，如果为1为满输出，对应SVPWM为满调制。
	         如果速度闭环，而编码器反馈有问题(或者程序有问题)，会导致
	         满输出，也就是高电压而烧坏电机或者功率板
	    !!!  用户试验过程中，根据实际情况修改
        -----------------------------------------------------------------
	 */
    pid1_id.OutMax = _IQ(0.05);//_IQ(0.30); 
    pid1_id.OutMin = _IQ(-0.05);//_IQ(-0.30);    
    
    pid1_id.Ui     = _IQ(0);
    pid1_id.Up     = _IQ(0);
    pid1_id.Up1    = _IQ(0);
    pid1_id.Out    = _IQ(0);
    pid1_id.OutPreSat= _IQ(0);
    
// Initialize the PID_REG3 module for Iq
	pid1_iq.Kp = _IQ(0.01);   //_IQ(0.1); 
	pid1_iq.Ki = _IQ(T/0.02);//_IQ(T/0.02);				
	pid1_iq.Kd = _IQ(0/T); 				
	pid1_iq.Kc = _IQ(0);//_IQ(0.5);

	
	/*  ----------------------    重要关注    ---------------------------
	
	    !!!  Iq的PID输出为标幺值，如果为1为满输出，对应SVPWM为满调制。
	         如果速度闭环，而编码器反馈有问题(或者程序有问题)，会导致
	         满输出，也就是高电压而烧坏电机或者功率板
	    !!!  用户试验过程中，如果各方配合正常，可以适当调高该值
        -----------------------------------------------------------------
	 */
    pid1_iq.OutMax = _IQ(0.1);//_IQ(0.95);
    pid1_iq.OutMin = _IQ(-0.1);//_IQ(-0.95);

    
    pid1_iq.Ui     = _IQ(0);
    pid1_iq.Up     = _IQ(0);
    pid1_iq.Up1    = _IQ(0);
    pid1_iq.Out    = _IQ(0);
    pid1_iq.OutPreSat= _IQ(0);
    
// Initialize the PID_REG3 module for speed control
    pid1_spd.Kp = _IQ(0.01);//_IQ(1);                  
	pid1_spd.Ki = _IQ(T*SpeedLoopPrescaler/0.3);//_IQ(T*SpeedLoopPrescaler/0.3);
	pid1_spd.Kd = _IQ(0/(T*SpeedLoopPrescaler));
 	pid1_spd.Kc = _IQ(0);//_IQ(0.2);
    pid1_spd.OutMax = _IQ(1);//_IQ(1);
    pid1_spd.OutMin = _IQ(-1);//_IQ(-1); 
    pid1_spd.Out    = _IQ(0);

// Initialize the PID_REG3 module for position control
    pid1_pos.Kp = _IQ(28.2);
	pid1_pos.Ki = _IQ(0);        // Integral term is not used 
	pid1_pos.Kd = _IQ(0);        // Derivative term is not used
 	pid1_pos.Kc = _IQ(0);
    pid1_pos.OutMax = _IQ(1);
    pid1_pos.OutMin = _IQ(-1);

// 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++;

}

void PMSM_Start(void)
{
    unsigned int i;

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

    bMode = PMSM_OPEN_LOOP;
    
    F2812_EV1_PWM_Init(&pwm1);
    //cap1.init(&cap1); 
    drv1.init(&drv1);

    // PWM DAC输出跟踪初始化
    EALLOW;

    #if 0
    pwmdac1.pwmdac_period = 2500;  /* PWM frequency = 30 kHz */
    pwmdac1.PWM_DAC_IPTR0 = &pwmdac_ch1;
    pwmdac1.PWM_DAC_IPTR1 = &pwmdac_ch2;
    pwmdac1.PWM_DAC_IPTR2 = &pwmdac_ch3;
	pwmdac1.init(&pwmdac1); 
	#endif
	
    EvaRegs.DBTCONA.all = DBTCON_INIT_STATE;
    EvaRegs.ACTRA.all   = ACTR_INIT_STATE;

    EvaRegs.EVAIMRA.bit.T1UFINT = 1;
    EvaRegs.EVAIFRA.bit.T1UFINT = 1;
    EDIS;

    DINT;
    PieCtrl.PIEIER2.bit.INTx6 = 1;   //  EVA Timer
    PieCtrl.PIEIER3.bit.INTx7 = 1;   //  Qep Isr
    IER |= ( M_INT2 | M_INT3 );
    EINT;


    EALLOW;
    GpioDataRegs.GPDDAT.bit.GPIOD1           = 0; // 清除驱动板故障保护
    EDIS;

    for (i = 0; i < 65000; i++) {NOP;}              // 延时等待故障清除

    EALLOW;
    GpioDataRegs.GPDDAT.bit.GPIOD1           = 1;
    GpioDataRegs.GPADAT.bit.GPIOA6 = 0;  /* IOPB4 low (T1PWM_GPIOA6) */
    EDIS;
}

void PMSM_Stop(void)
{
    bMode = PMSM_OPEN_LOOP;
    
    EALLOW;
    EvaRegs.EVAIMRA.bit.T1UFINT = 0;
    EvaRegs.EVAIFRA.bit.T1UFINT = 0;
    EDIS;

    DINT;
    PieCtrl.PIEIER2.bit.INTx6 = 0;   //  EVA Timer
    PieCtrl.PIEIER3.bit.INTx7 = 0;   //  Qep Isr
    EINT;

    // 关闭PWM输出, 强制为高
    EALLOW;
    EvaRegs.ACTRA.all = 0x0FFF; 
    GpioDataRegs.GPADAT.bit.GPIOA6 = 1;  /* IOPB4 high (T1PWM_GPIOA6) */
    EDIS;

    BEEP_Off();
}

void PMSM_SetRpm(float fSpeedHz)
{
    if (10 <= fSpeedHz)
    {
        SpeedRef = fSpeedHz;
    }
    else
    {
        SpeedRef = fSpeedHz;
    }
}

void PMSM_SetDir(float fDir)
{
    if (1 <= fDir)
    {
        rg1.Gain = 1;
    }
    else if (-1 >= fDir)
    {
        rg1.Gain = -1;
    }
}

void pid_Iq_calc(PIDREG3 *v)
{	
    // Compute the error
    v->Err = v->Ref - v->Fdb;

    // Compute the proportional output
    v->Up = _IQmpy(v->Kp,v->Err);

    fIa = _IQtoF(v->Up);

    // Compute the integral output
    v->Ui = v->Ui + _IQmpy(v->Ki,v->Up) + _IQmpy(v->Kc,v->SatErr);

    fIa = _IQtoF(v->Ui);

    // Compute the derivative output
    v->Ud = _IQmpy(v->Kd,(v->Up - v->Up1));

    fIa = _IQtoF(v->Ud);

    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + v->Ud;

    fIa = _IQtoF(v->OutPreSat);

    // Saturate the output
    if (v->OutPreSat > v->OutMax)
      v->Out =  v->OutMax;
    else if (v->OutPreSat < v->OutMin)
      v->Out =  v->OutMin;
    else
      v->Out = v->OutPreSat;

    fIa = _IQtoF(v->Out);
    
    // Compute the saturate difference
    v->SatErr = v->Out - v->OutPreSat;

    // Update the previous proportional output
    v->Up1 = v->Up; 

}

interrupt void PmsmISR(void)
{
    _iq19 iq19Vdc;
    _iq19 iq19Iu, iq19Iv, iq19Iw;
    short usTmp;

// Verifying the ISR
    IsrTicker++;

    //if (10 == IsrTicker) BEEP_On();
    if (25000 <IsrTicker)
    {
        IsrTicker = 0;
    }

    usTmp  = asOscSamData[ADC_CHAN_Iv_INDEX]-2048;
  	iq19Iv = _IQ19(usTmp);
  	iq19Iv = _IQ19mpy(iq19Iv, _IQ19(FLOAT_FACTOR_Iv) );

    usTmp  = asOscSamData[ADC_CHAN_Iw_INDEX]-2048;
  	iq19Iw = _IQ19(usTmp);
    iq19Iw = _IQ19mpy(iq19Iw, _IQ19(FLOAT_FACTOR_Iw) );
    
    usTmp  =  -asOscSamData[ADC_CHAN_Iv_INDEX] - asOscSamData[ADC_CHAN_Iw_INDEX] + 4096;
  	iq19Iu = _IQ19(usTmp);
    iq19Iu = _IQ19mpy(iq19Iu, _IQ19(FLOAT_FACTOR_Iu) );


    //fIa    = _IQ19toF(iq19Iu);
    //fIb    = _IQ19toF(iq19Iv);
    //fIc    = _IQ19toF(iq19Iw);
    
// ***************** LEVEL1 *****************
//#if (BUILDLEVEL==LEVEL1)

    if (PMSM_OPEN_LOOP == bMode)
    {
    // ------------------------------------------------------------------------------
    //    Connect inputs of the RMP module and call the Ramp control
    //    calculation function.
    // ------------------------------------------------------------------------------
        rc1.TargetValue = _IQ(SpeedRef);
        rc1.calc(&rc1);

    // ------------------------------------------------------------------------------
    //    Connect inputs of the RAMP GEN module and call the Ramp generator
    //    calculation function.
    // ------------------------------------------------------------------------------
        rg1.Freq = rc1.SetpointValue;
        rg1.calc(&rg1);

    // ------------------------------------------------------------------------------
    //    Connect inputs of the INV_PARK module and call the inverse park transformation
    //    calculation function.
    // ------------------------------------------------------------------------------
        ipark1.Ds = _IQ(VdTesting); //_IQ(VdTesting);
        ipark1.Qs = _IQmpy(rg1.Freq, _IQ(0.02)); // vi= Kf = V/200 * f = V * 0.005 * f; //_IQ(VqTesting);	
        ipark1.Angle = rg1.Out;
        ipark1.calc(&ipark1);

    // ------------------------------------------------------------------------------
    //    Connect inputs of the SVGEN_DQ module and call the space-vector gen.
    //    calculation function.
    // ------------------------------------------------------------------------------
      	svgen_dq1.Ualpha = ipark1.Alpha;
     	svgen_dq1.Ubeta = ipark1.Beta;
      	svgen_dq1.calc(&svgen_dq1);	

    // ------------------------------------------------------------------------------
    //    Connect inputs of the PWM_DRV module and call the PWM signal generation 
    //    update function.
    // ------------------------------------------------------------------------------
        pwm1.MfuncC1 = (int16)_IQtoIQ15(svgen_dq1.Ta); // MfuncC1 is in Q15
        pwm1.MfuncC2 = (int16)_IQtoIQ15(svgen_dq1.Tb); // MfuncC2 is in Q15  
        pwm1.MfuncC3 = (int16)_IQtoIQ15(svgen_dq1.Tc); // MfuncC3 is in Q15
    	pwm1.update(&pwm1);

    	qep1.calc(&qep1);
        speed1.ElecTheta = _IQ15toIQ((int32)qep1.ElecTheta);
        speed1.DirectionQep = (int32)(qep1.DirectionQep);
        speed1.calc(&speed1);
    // ------------------------------------------------------------------------------
    //    Connect inputs of the PWMDAC module 
    // ------------------------------------------------------------------------------	
        PwmDacCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
        PwmDacCh2 = (int16)_IQtoIQ15(svgen_dq1.Tb);    
        PwmDacCh3 = (int16)_IQtoIQ15(svgen_dq1.Tc);    

    // ------------------------------------------------------------------------------
    //    Connect inputs of the DATALOG module 
    // ------------------------------------------------------------------------------
        DlogCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
        DlogCh2 = (int16)_IQtoIQ15(svgen_dq1.Tb);
        DlogCh3 = (int16)_IQtoIQ15(svgen_dq1.Tc);
        DlogCh4 = (int16)_IQtoIQ15(svgen_dq1.Ta-svgen_dq1.Tb);

    // ------------------------------------------------------------------------------
    //    Connect inputs of the EN_DRV module and call the enable/disable PWM signal
    //    update function. (FOR DMC1500 ONLY)
    // ------------------------------------------------------------------------------ 
        #if 0  // 使用计算机 Motor Monitor 软件观测，不需要
        drv1.EnableFlag = EnableFlag;
        drv1.update(&drv1);
        #endif
    }

//#endif // (BUILDLEVEL==LEVEL1)


// ***************** LEVEL2 *****************
#if (BUILDLEVEL==LEVEL2)

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP module and call the Ramp control
//    calculation function.
// ------------------------------------------------------------------------------
    rc1.TargetValue = _IQ(SpeedRef);
    rc1.calc(&rc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the RAMP GEN module and call the Ramp generator
//    calculation function.
// ------------------------------------------------------------------------------
    rg1.Freq = rc1.SetpointValue;
    rg1.calc(&rg1);

// ------------------------------------------------------------------------------
//    Call the ILEG2_VDC read function.
// ------------------------------------------------------------------------------
    ilg2_vdc1.read(&ilg2_vdc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the CLARKE module and call the clarke transformation
//    calculation function.
// ------------------------------------------------------------------------------
 	clarke1.As = _IQ15toIQ((int32)ilg2_vdc1.ImeasA);