acim.c

使用dsPIC30F 实现交流感应电机的矢量控制源代码

        
        // If the application is running in torque mode, the velocity
        // control loop is bypassed.  The velocity reference value, read
        // from the potentiometer, is used directly as the torque 
        // reference, VqRef.
        #ifdef	TORQUE_MODE
        CtrlParm.qVqRef    = CtrlParm.qVelRef;
        
        #else
        // Check to see if new velocity information is available by comparing
        // the number of interrupts per velocity calculation against the
        // number of velocity count samples taken.  If new velocity info
        // is available, calculate the new velocity value and execute
        // the speed control loop.
        if(EncoderParm.iVelCntDwn == EncoderParm.iIrpPerCalc)
        	{
	        // Calculate velocity from acumulated encoder counts
        	CalcVel();
        	// Execute the velocity control loop
        	PIParmQref.qInMeas = EncoderParm.qVelMech;
        	PIParmQref.qInRef  = CtrlParm.qVelRef;
        	CalcPI(&PIParmQref);
        	CtrlParm.qVqRef    = PIParmQref.qOut;
        	}
        #endif
       
        // PI control for Q
        PIParmQ.qInMeas = ParkParm.qIq;
        PIParmQ.qInRef  = CtrlParm.qVqRef;
        CalcPI(&PIParmQ);
        ParkParm.qVq    = PIParmQ.qOut;

        // PI control for D
        PIParmD.qInMeas = ParkParm.qId;
        PIParmD.qInRef  = CtrlParm.qVdRef;
        CalcPI(&PIParmD);
        ParkParm.qVd    = PIParmD.qOut;
        
        
        }
}

//---------------------------------------------------------------------
// The ADC ISR does speed calculation and executes the vector update loop.
// The ADC sample and conversion is triggered by the PWM period.
// The speed calculation assumes a fixed time interval between calculations.
//---------------------------------------------------------------------

void __attribute__((__interrupt__)) _ADCInterrupt(void)
{
        IFS0bits.ADIF = 0;
        
        // Increment count variable that controls execution
        // of display and button functions.
        iDispLoopCnt++;
        
        // acumulate encoder counts since last interrupt  
        CalcVelIrp();  
     
        if( uGF.bit.RunMotor )
                {
                
                // Set LED1 for diagnostics
                pinLED1 = 1; 
                
                // use TMR1 to measure interrupt time for diagnostics
                TMR1 = 0;           
                iLoopCnt = TMR1;
		 
		        // Calculate qIa,qIb
                MeasCompCurr();
                
                // Calculate qId,qIq from qSin,qCos,qIa,qIb
                ClarkePark();
                               
                // Calculate control values
                DoControl();
							
                // Calculate qSin,qCos from qAngle
                SinCos();

                // Calculate qValpha, qVbeta from qSin,qCos,qVd,qVq
                InvPark();    

                // Calculate Vr1,Vr2,Vr3 from qValpha, qVbeta 
                CalcRefVec();

                // Calculate and set PWM duty cycles from Vr1,Vr2,Vr3
                CalcSVGen();
                                  
                // Measure loop time
                iLoopCnt = TMR1 - iLoopCnt;
                if( iLoopCnt > iMaxLoopCnt )
                    iMaxLoopCnt = iLoopCnt;
                               
                // Clear LED1 for diagnostics
                pinLED1 = 0;
                }    
}

//---------------------------------------------------------------------
// SetupBoard
//
// Initialze board
//---------------------------------------------------------------------

void SetupBoard( void )
{
    BYTE b;

    // Disable ADC interrupt
    IEC0bits.ADIE = 0; 

    // Reset any active faults on the motor control power module.
    pinFaultReset = 1;
    for(b=0;b<10;b++)
        Nop();
    pinFaultReset = 0;


    // Ensure PFC switch is off.
    pinPFCFire = 0;
    // Ensure brake switch is off.
    pinBrakeFire = 0;
}

//---------------------------------------------------------------------
// Dis_RPM
//
// Display RPM
//---------------------------------------------------------------------

void Dis_RPM( BYTE bChrPosC, BYTE bChrPosR )
{

	if (EncoderParm.iDeltaCnt < 0)
	    Wrt_S_LCD("-", bChrPosC, bChrPosR);
	else
	    Wrt_S_LCD(" ", bChrPosC, bChrPosR);
	       
    iRPM = EncoderParm.iDeltaCnt*60/(MotorParm.fLoopPeriod*MotorParm.iIrpPerCalc*EncoderParm.iCntsPerRev);
    Wrt_Signed_Int_LCD( iRPM, bChrPosC+1, bChrPosR);
}    
//---------------------------------------------------------------------
bool SetupParm(void)
{
    // Turn saturation on to insure that overflows will be handled smoothly.
    CORCONbits.SATA  = 0;

    // Setup required parameters

	// Pick scaling values to be 8 times nominal for speed and current
	
	// Use 8 times nominal mechanical speed of motor (in RPM) for scaling
    MotorParm.iScaleMechRPM  = diNomRPM*8;
    
    // Number of pole pairs
    MotorParm.iPoles        = diPoles ;

    // Encoder counts per revolution as detected by the
    //       dsPIC quadrature configuration.
    MotorParm.iCntsPerRev  = diCntsPerRev;    
	
	// Rotor time constant in sec
    MotorParm.fRotorTmConst = dfRotorTmConst;

    // Basic loop period (in sec).  (PWM interrupt period)
    MotorParm.fLoopPeriod = dLoopInTcy * dTcy;  //Loop period in cycles * sec/cycle

    // Encoder velocity interrupt period (in sec). 
    MotorParm.fVelIrpPeriod = MotorParm.fLoopPeriod;

    // Number of vel interrupts per velocity calculation.
    MotorParm.iIrpPerCalc = diIrpPerCalc;       // In loops 

    // Scale mechanical speed of motor (in rev/sec)
    MotorParm.fScaleMechRPS = MotorParm.iScaleMechRPM/60.0;

    // Scaled flux speed of motor (in rev/sec)
    // All dimensionless flux velocities scaled by this value.
    MotorParm.fScaleFluxRPS = MotorParm.iPoles*MotorParm.fScaleMechRPS;

    // Minimum period of one revolution of flux vector (in sec)
    MotorParm.fScaleFluxPeriod = 1.0/MotorParm.fScaleFluxRPS;

    // Fraction of revolution per LoopTime at maximum flux velocity
    MotorParm.fScaleFracRevPerLoop = MotorParm.fLoopPeriod * MotorParm.fScaleFluxRPS; 

    // Scaled flux speed of motor (in radians/sec)
    // All dimensionless velocities in radians/sec scaled by this value.
    MotorParm.fScaleFluxSpeed = 6.283 * MotorParm.fScaleFluxRPS;

    // Encoder count rate at iScaleMechRPM
    MotorParm.lScaleCntRate = MotorParm.iCntsPerRev * (MotorParm.iScaleMechRPM/60.0);



// ============= Open Loop ======================

    OpenLoopParm.qKdelta = 32768.0 * 2 * MotorParm.iPoles * MotorParm.fLoopPeriod * MotorParm.fScaleMechRPS;
    OpenLoopParm.qVelMech = dqOL_VelMech;    
    CtrlParm.qVelRef = OpenLoopParm.qVelMech;

    InitOpenLoop();

// ============= Encoder ===============

    if( InitEncoderScaling() )
        // Error
        return True;

// ============= ADC - Measure Current & Pot ======================

    // Scaling constants: Determined by calibration or hardware design.
    ReadADCParm.qK      = dqK;    

    MeasCurrParm.qKa    = dqKa;    
    MeasCurrParm.qKb    = dqKb;   

    // Inital offsets
    InitMeasCompCurr( 450, 730 );    

// ============= Current Model ===============

    if(InitCurModelScaling())
        // Error
        return True;

// ============= Field Weakening ===============
    // Field Weakening constant for constant torque range
    FdWeakParm.qK1 = dqK1;       // Flux reference value

// ============= PI D Term ===============      
    PIParmD.qKp = dDqKp;       
    PIParmD.qKi = dDqKi;              
    PIParmD.qKc = dDqKc;       
    PIParmD.qOutMax = dDqOutMax;
    PIParmD.qOutMin = -PIParmD.qOutMax;

    InitPI(&PIParmD);

// ============= PI Q Term ===============
    PIParmQ.qKp = dQqKp;    
    PIParmQ.qKi = dQqKi;
    PIParmQ.qKc = dQqKc;
    PIParmQ.qOutMax = dQqOutMax;
    PIParmQ.qOutMin = -PIParmQ.qOutMax;

    InitPI(&PIParmQ);

// ============= PI Qref Term ===============
    PIParmQref.qKp = dQrefqKp;       
    PIParmQref.qKi = dQrefqKi;       
    PIParmQref.qKc = dQrefqKc;       
    PIParmQref.qOutMax = dQrefqOutMax;   
    PIParmQref.qOutMin = -PIParmQref.qOutMax;

    InitPI(&PIParmQref);

// ============= SVGen ===============
    // Set PWM period to Loop Time 
    SVGenParm.iPWMPeriod = dLoopInTcy;      

// ============= TIMER #1 ======================
    PR1 = 0xFFFF;
    T1CONbits.TON = 1;
    T1CONbits.TCKPS = 1;    // prescale of 8 => 1.08504 uS tick

// ============= Motor PWM ======================

    PDC1 = 0;
    PDC2 = 0;
    PDC3 = 0;
    PDC4 = 0;

    // Center aligned PWM.
    // Note: The PWM period is set to dLoopInTcy/2 but since it counts up and 
    // and then down => the interrupt flag is set to 1 at zero => actual 
    // interrupt period is dLoopInTcy

    PTPER = dLoopInTcy/2;   // Setup PWM period to Loop Time defined in parms.h 

    PWMCON1 = 0x0077;       // Enable PWM 1,2,3 pairs for complementary mode
    DTCON1 = dDeadTime;     // Dead time
    DTCON2 = 0;
    FLTACON = 0;            // PWM fault pins not used
    FLTBCON = 0;
    PTCON = 0x8002;         // Enable PWM for center aligned operation

    // SEVTCMP: Special Event Compare Count Register 
    // Phase of ADC capture set relative to PWM cycle: 0 offset and counting up
    SEVTCMP = 2;        // Cannot be 0 -> turns off trigger (Missing from doc)       
    SEVTCMPbits.SEVTDIR = 0;

// ============= Encoder ===============

    MAXCNT = MotorParm.iCntsPerRev;    
    POSCNT = 0;
    QEICON = 0;
    QEICONbits.QEIM = 7;    // x4 reset by MAXCNT pulse
    QEICONbits.POSRES = 0;  // Don't allow Index pulse to reset counter
    QEICONbits.SWPAB = 0;   // direction 
    DFLTCON = 0;            // Digital filter set to off

// ============= ADC - Measure Current & Pot ======================
// ADC setup for simultanous sampling on 
//      CH0=AN7, CH1=AN0, CH2=AN1, CH3=AN2. 
// Sampling triggered by PWM and stored in signed fractional form.

    ADCON1 = 0;
    // Signed fractional (DOUT = sddd dddd dd00 0000)
    ADCON1bits.FORM = 3;    
    // Motor Control PWM interval ends sampling and starts conversion
    ADCON1bits.SSRC = 3;  
    // Simultaneous Sample Select bit (only applicable when CHPS = 01 or 1x)
    // Samples CH0, CH1, CH2, CH3 simultaneously (when CHPS = 1x)
    // Samples CH0 and CH1 simultaneously (when CHPS = 01)
    ADCON1bits.SIMSAM = 1;  
    // Sampling begins immediately after last conversion completes. 
    // SAMP bit is auto set.
    ADCON1bits.ASAM = 1;  


    ADCON2 = 0;
    // Samples CH0, CH1, CH2, CH3 simultaneously (when CHPS = 1x)
    ADCON2bits.CHPS = 2;  


    ADCON3 = 0;
    // A/D Conversion Clock Select bits = 8 * Tcy
    ADCON3bits.ADCS = 15;  


    /* ADCHS: ADC Input Channel Select Register */
    ADCHS = 0;
    // CH0 is AN7
    ADCHSbits.CH0SA = 7;
    // CH1 positive input is AN0, CH2 positive input is AN1, CH3 positive input is AN2
    ADCHSbits.CH123SA = 0;


    /* ADPCFG: ADC Port Configuration Register */
    // Set all ports digital
    ADPCFG = 0xFFFF;
    ADPCFGbits.PCFG0 = 0;   // AN0 analog
    ADPCFGbits.PCFG1 = 0;   // AN1 analog
    ADPCFGbits.PCFG2 = 0;   // AN2 analog
    ADPCFGbits.PCFG7 = 0;   // AN7 analog

    /* ADCSSL: ADC Input Scan Select Register */
    ADCSSL = 0;

    // Turn on A/D module
    ADCON1bits.ADON = 1;
    
    #ifdef	DIAGNOSTICS
    // Initialize Output Compare 7 and 8 for use in diagnostics.
    // Compares are used in PWM mode
    // Timer2 is used as the timebase
    PR2 = 0x1FFF;
    OC7CON = 0x0006;
    OC8CON = 0x0006;
    T2CONbits.TON = 1;
    #endif
    

    return False;
}

#ifdef	DIAGNOSTICS
void DiagnosticsOutput(void)
{
int Data;

	if(IFS0bits.T2IF)
		{
		IFS0bits.T2IF = 0;		
		Data = (ParkParm.qIq >> 4) + 0xfff;
		if(Data > 0x1ff0) Data = 0x1ff0;
		if(Data < 0x000f) Data = 0x000f;
		OC7RS = Data;
		Data =  (EncoderParm.qVelMech) + 0x0fff;
		if(Data > 0x1ff0) Data = 0x1ff0;
		if(Data < 0x000f) Data = 0x000f;
		OC8RS = Data;
		}	
	
}
#endif