SnapShotDelayCnt = 0;
			// Log data in the snapshot buffers
	       	if( uGF.bit.DoSnap )
	        {
		     SnapBuf1[SnapCount] = SNAP1;
		     SnapBuf2[SnapCount] = SNAP2;  
		     SnapBuf3[SnapCount] = SNAP3;   
		     SnapCount++;    
	         if(SnapCount == SNAPSIZE)
	            {
					SnapCount = 0;
	                uGF.bit.SnapDone=1;
	                uGF.bit.DoSnap = 0;
	            }
	         }
   		}
       	#endif
       
       
        // 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.
       	
        // 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)
{
		_RG0 = 1;
        IFS0bits.ADIF = 0;
        
        // Increment count variable that controls execution
        // of display and button functions.
        iDispLoopCnt++;
        
        // acumulate position 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 qAngle from QEI Module
				CalculateParkAngle();

                // 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;
                }    
		_RG0 = 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 (FilteredOmega < 0)
	    Wrt_S_LCD("-", bChrPosC, bChrPosR);
	else
	    Wrt_S_LCD("+", bChrPosC, bChrPosR);
	
	iRPM_old = iRPM;
	iRPM = (int)((float)(FilteredOmega / 32768.0) * 60.0) / \
           (SPEED_CONSTANT * diIrpPerCalc * diPoles * dLoopTimeInSec * 2.0);
    iRPM = (iRPM + iRPM_old) >> 1;
    Wrt_Signed_Int_LCD( iRPM, bChrPosC, bChrPosR);

	if (__OffsetTheta < 0)
	    Wrt_S_LCD("-", bChrPosC+6, bChrPosR);
	else
	    Wrt_S_LCD("+", bChrPosC+6, bChrPosR);
	
	iOffset_old = iOffset;       
    iOffset = __OffsetTheta/90;
    iOffset = (iOffset + iOffset_old) >> 1;
	Wrt_Signed_Int_LCD(iOffset, bChrPosC+7, bChrPosR);
}    

//---------------------------------------------------------------------
bool SetupParm(void)
{
    // Turn saturation on to insure that overflows will be handled smoothly.
    CORCONbits.SATA  = 0;

    // Setup required parameters
 
    // Number of pole pairs
    MotorParm.iPoles        = diPoles ;

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

    // 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 ======================
	// Motor End Speed Calculation

	MotorParm.EndSpeed = END_SPEED_RPM * MotorParm.iPoles * dLoopTimeInSec * 65536 / 60.0;
	MotorParm.EndSpeed = MotorParm.EndSpeed * 1024;
	MotorParm.LockTime = LOCK_TIME;

// ============= 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(0,0);    

// ============= 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 = (0x40) | (dDeadTime / 2);     // Dead time
    DTCON2 = 0;
    FLTACON = 0;            // PWM fault pins not used
    FLTBCON = 0;

	IFS2bits.PWMIF = 0;
	IEC2bits.PWMIE = 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)       
	SEVTCMP = PTPER;
    SEVTCMPbits.SEVTDIR = 0;

// ============= 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;
    
    return False;
}

void CalculateParkAngle(void)
{
 	smc1.Ialpha = ParkParm.qIalpha;
  	smc1.Ibeta = ParkParm.qIbeta;
    smc1.Valpha = ParkParm.qValpha;
    smc1.Vbeta = ParkParm.qVbeta;

	SMC_Position_Estimation(&smc1);

	if(uGF.bit.OpenLoop)	
	{
		if (Startup_Lock < LOCK_TIME)
			Startup_Lock+=1;
		else if (Startup_Ramp < MotorParm.EndSpeed)
			Startup_Ramp+=1;
		else
		{
            uGF.bit.ChangeMode = 1;
            uGF.bit.OpenLoop = 0;
            pinLED2 = !uGF.bit.OpenLoop;
		}
		ParkParm.qAngle += (int)(Startup_Ramp >> 10);
	}
	else
	{
		ParkParm.qAngle = smc1.Theta;
	}
	return;
}

void SetupControlParameters(void)
{

// ============= 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);
	return;
}