acim.c

dspic30f6010a的学习事例程序

                // Calculate qSin,qCos from qAngle
                SinCos();

// Appcon/Changes 15 dec 2006
//****************************
				ParkParm.qSin=SincosParm.qSin;
				ParkParm.qCos=SincosParm.qCos;
//****************************

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

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_old = iRPM; 

// Appcon/Changes 25 Jan 2007. Estimated speed is displayed 
// when sensorless control is selected
// iRPM= 0,8838*EstimParm.qVelEstim
//****************************                
	if (uGF.bit.Sensorless)      
	{ 
    	iRPM = ((long)EstimParm.qVelEstimMech*(long)0x7120)>>15;
//**************************** 
	} else
	{
		iRPM = EncoderParm.iDeltaCnt*60/(MotorParm.fLoopPeriod*MotorParm.iIrpPerCalc*EncoderParm.iCntsPerRev);
	}
    iRPM = (iRPM + iRPM_old) >> 1;
    Wrt_Signed_Int_LCD( iRPM, bChrPosC+1, bChrPosR);
}    
//---------------------------------------------------------------------

void InitUserParms(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();
		
	// ============= 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);
    
    // ============= ADC - Measure Current & Pot ======================

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

    MeasCurrParm.qKa    = dqKa;    
    MeasCurrParm.qKb    = dqKb;  
// Appcon/Changes 25 Jan 2007
//*******************************
    MeasCurrParm.iOffsetCompVelLimit= iCompVelLimitLow;
    MeasCurrParm.iOffsetCompHyst=iCompVelHyst;
    MeasCurrParm.iOffsetCompAktiv=0;
	CtrlParm.qVelRef=0;	// Spedd reference value	

//*******************************

    // Inital offsets
    InitMeasCompCurr( 450, 730 );  
	
}




//---------------------------------------------------------------------
bool SetupPeripherals(void)
{
    

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

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

  

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

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



// ============= 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 = 1;   // 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;
//		Data = (EstimParm.qRho >> 4) + 0xfff;
		if(Data > 0x1ff0) Data = 0x1ff0;
		if(Data < 0x000f) Data = 0x000f;
		OC7RS = Data;
		Data =  (EncoderParm.qVelMech) + 0x0fff;
//		Data =  (CurModelParm.qAngFlux) + 0x0fff;
		if(Data > 0x1ff0) Data = 0x1ff0;
		if(Data < 0x000f) Data = 0x000f;
		OC8RS = Data;
		}	
	
}
#endif