pmsm3_4_4.c

电机矢量控制的程序。自己调试成功的。包括svpwm

    ilg2_meas1.read(&ilg2_meas1);

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

// ------------------------------------------------------------------------------
//    Connect inputs of the PARK module and call the park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    park1.Alpha = clarke1.Alpha;
    park1.Beta = clarke1.Beta;
    park1.Angle = _IQ15toIQ((int32)qep1.ElecTheta);
    park1.calc(&park1);
// ------------------------------------------------------------------------------
//	  Connect inputs of the PID_REG4 module and call the PID position controller
//    calculation function.
// ------------------------------------------------------------------------------
//    pid_pos.Ref = _IQ(PositionRef);
    pid1_pos.Ref = ramp_ctl1.SetpointValue;
    pid1_pos.Fdb = _IQ15toIQ((int32)qep1.MechTheta);
	pid1_pos.calc(&pid1_pos);
 
// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID speed controller
//    calculation function.
// ------------------------------------------------------------------------------  
    if (SpeedLoopCount==SpeedLoopPrescaler)
     {
      pid1_spd.Ref = pid1_pos.Out;
      pid1_spd.Fdb = speed1.Speed;
	  pid1_spd.calc(&pid1_spd);
      SpeedLoopCount=1;
     }
    else SpeedLoopCount++; 

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID IQ controller
//    calculation function.
// ------------------------------------------------------------------------------  
    pid1_iq.Ref = pid1_spd.Out;
	pid1_iq.Fdb = park1.Qs;
	pid1_iq.calc(&pid1_iq);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID ID controller
//    calculation function.
// ------------------------------------------------------------------------------  
    pid1_id.Ref = _IQ(IdRef);
	pid1_id.Fdb = park1.Ds;
	pid1_id.calc(&pid1_id);

// ------------------------------------------------------------------------------
//    Connect inputs of the INV_PARK module and call the inverse park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    ipark1.Ds = pid1_id.Out;
    ipark1.Qs = pid1_iq.Out;	
    ipark1.Angle = speed1.ElecTheta;
    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);
	
// ------------------------------------------------------------------------------
//    Call the QEP calculation function 
// ------------------------------------------------------------------------------
    qep1.calc(&qep1);

// ------------------------------------------------------------------------------
//    Connect inputs of the SPEED_FR module and call the speed calculation function 
// ------------------------------------------------------------------------------

    #if (DSP_TARGET==F2812)
       speed1.ElecTheta = _IQ15toIQ((int32)qep1.ElecTheta);
       speed1.DirectionQep = (int32)(qep1.DirectionQep);
       speed1.calc(&speed1);
    #endif
    
// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
    PwmDacCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    PwmDacCh2 = (int16)_IQtoIQ15(speed1.ElecTheta);    
    PwmDacCh3 = (int16)_IQtoIQ15(clarke1.As);    

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
    DlogCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);
    DlogCh2 = (int16)_IQtoIQ15(speed1.ElecTheta);
    DlogCh3 = (int16)_IQtoIQ15(pid1_spd.Ref);
    DlogCh4 = (int16)_IQtoIQ15(pid1_spd.Fdb);

// ------------------------------------------------------------------------------
//    Connect inputs of the EN_DRV module and call the enable/disable PWM signal
//    update function. (FOR DMC1500 ONLY)
// ------------------------------------------------------------------------------ 
    drv1.EnableFlag = EnableFlag;
    drv1.update(&drv1);

#endif // (BUILDLEVEL==LEVEL5) 


// &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
//
// ***************** LEVEL6 *****************
//
// &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
#if (BUILDLEVEL==LEVEL6)

// ------------------------------------------------------------------------------
//    Specify the initial position reference when DC-bus voltage is less than 25%
// ------------------------------------------------------------------------------
//      if (_IQ15toIQ((int32)ilg2_vdc1.VdcMeas) < _IQ(0.25))
//         PositionRef = _IQtoF(_IQ15toIQ((int32)qep1.MechTheta));

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP_CNTL module and call the Ramp control
//    calculation function.
// ------------------------------------------------------------------------------
      ramp_ctl1.TargetValue = _IQ(PositionRef);
      ramp_ctl1.calc(&ramp_ctl1);

// ------------------------------------------------------------------------------
//    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_meas1.ImeasA);
     clarke1.Bs = _IQ15toIQ((int32)ilg2_meas1.ImeasB);
	 clarke1.calc(&clarke1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PARK module and call the park transformation
//    calculation function.
// ------------------------------------------------------------------------------
     park1.Alpha = clarke1.Alpha;
     park1.Beta = clarke1.Beta;

  #if (DSP_TARGET==F2812)
     park1.Angle = _IQ15toIQ((int32)qep1.ElecTheta);
  #endif
     park1.calc(&park1);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 modules and compute the P position 
//    controller. 
//    Note that this P controller is manually implemented in the main function.
//    It is not from the DMC library. 
// ------------------------------------------------------------------------------  
//   pid1_pos.Ref = _IQ(PositionRef); 
     pid1_pos.Ref = ramp_ctl1.SetpointValue;

  #if (DSP_TARGET==F2812)
     pid1_pos.Fdb = _IQ15toIQ((int32)qep1.MechTheta);
  #endif
     pid1_pos.calc(&pid1_pos);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID IQ controller
//    calculation function.
// ------------------------------------------------------------------------------  
    pid1_iq.Ref = pid1_pos.Out;
	pid1_iq.Fdb = park1.Qs;
	pid1_iq.calc(&pid1_iq);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID ID controller
//    calculation function.
// ------------------------------------------------------------------------------  
    pid1_id.Ref = _IQ(IdRef);
	pid1_id.Fdb = park1.Ds;
	pid1_id.calc(&pid1_id);

// ------------------------------------------------------------------------------
//    Call the QEP_DRV calculation function.
// ------------------------------------------------------------------------------
    qep1.calc(&qep1);

// ------------------------------------------------------------------------------
//    Connect inputs of the INV_PARK module and call the inverse park transformation
//    calculation function.
// ------------------------------------------------------------------------------
    ipark1.Ds = pid1_id.Out;
    ipark1.Qs = pid1_iq.Out;	

  #if (DSP_TARGET==F2812)
    ipark1.Angle = _IQ15toIQ((int32)qep1.ElecTheta);
  #endif
    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);
    
// ------------------------------------------------------------------------------
//    Connect inputs of the PWMDAC module 
// ------------------------------------------------------------------------------	
    PwmDacCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);

  #if (DSP_TARGET==F2812)
    PwmDacCh2 = qep1.ElecTheta;
  #endif
    PwmDacCh3 = (int16)_IQtoIQ15(clarke1.As);    

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
    DlogCh1 = (int16)_IQtoIQ15(svgen_dq1.Ta);

  #if (DSP_TARGET==F2812)
    DlogCh3 = qep1.ElecTheta;
  #endif
    DlogCh3 = (int16)_IQtoIQ15(pid1_pos.Ref);
    DlogCh4 = (int16)_IQtoIQ15(pid1_pos.Fdb);

// ------------------------------------------------------------------------------
//    Connect inputs of the EN_DRV module and call the enable/disable PWM signal
//    update function. (FOR DMC1500 ONLY)
// ------------------------------------------------------------------------------ 
    drv1.EnableFlag = EnableFlag;
    drv1.update(&drv1);

#endif // (BUILDLEVEL==LEVEL6)


// ------------------------------------------------------------------------------
//    Call the PWMDAC update function.
// ------------------------------------------------------------------------------
	pwmdac1.update(&pwmdac1);  

// ------------------------------------------------------------------------------
//    Call the DATALOG update function.
// ------------------------------------------------------------------------------
    dlog.update(&dlog);



#if (DSP_TARGET==F2812)
// Enable more interrupts from this timer
	EvaRegs.EVAIMRA.bit.T1UFINT = 1;
	
// Note: To be safe, use a mask value to write to the entire
	// EVAIFRA register.  Writing to one bit will cause a read-modify-write
	// operation that may have the result of writing 1's to clear 
	// bits other then those intended. 
    EvaRegs.EVAIFRA.all = BIT9;
	
// Acknowledge interrupt to recieve more interrupts from PIE group 2
	PieCtrlRegs.PIEACK.all |= PIEACK_GROUP2;
#endif

}

//++++++++++++++++++++++++++++++++++++++++++++++++++++



//++++++++++++++++++++++++++++++++++++++++++++++++++++
//                   QepISR()
//++++++++++++++++++++++++++++++++++++++++++++++++++++
#if (DSP_TARGET==F2812)
interrupt void QepISR(void)
{
	QepISR_CNT++;
// ------------------------------------------------------------------------------
//    Call the QEP_DRV isr function.
// ------------------------------------------------------------------------------
   qep1.isr(&qep1);

// Enable more interrupts from this timer
	EvaRegs.EVAIMRC.bit.CAP3INT = 1;
	
// Note: To be safe, use a mask value to write to the entire
	// EVAIFRC register.  Writing to one bit will cause a read-modify-write
	// operation that may have the result of writing 1's to clear 
	// bits other then those intended. 
    EvaRegs.EVAIFRC.all = BIT2;
	
// Acknowledge interrupt to recieve more interrupts from PIE group 3
	PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
  
}
#endif

//++++++++++++++++++++++++++++++++++++++++++++++++++++
//                   PdpIntAISR()
//++++++++++++++++++++++++++++++++++++++++++++++++++++

interrupt void PdpIntAISR( void )    // EV-A,
{
  	PdpIntAISR_CNT++;
    // Disable Full Compare Output:
	// EvaRegs.COMCONA.bit.FCOMPOE = 0; // 编译出错！0 Full compare outputs, PWM1/2/3/4/5/6, are in Hi-Z state.
	EvaRegs.COMCONA.all &= 0xfdff;   //等价于EvaRegs.COMCONA.bit.FCOMPOE = 0;
	// For test this routine
//    printf("This is PDPINTA_ISR!!\n"); 

	//DisableVCC_OTP();         //驱动IPM光耦的控制端电源断电

	// Enable more interrupts from this timer
 	EvaRegs.EVAIMRA.bit.PDPINTA = 1;

  // Note: To be safe, use a mask value to write to the entire
	// EVAIFRA register.  Writing to one bit will cause a read-modify-write
	// operation that may have the result of writing 1's to clear 
	// bits other then those intended. 
    EvaRegs.EVAIFRA.all = BIT0;  
  
  // To receive more interrupts from this PIE group, acknowledge this interrupt 
   	PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

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