/**********************************************************************
* © 2012 Microchip Technology Inc.
*
* SOFTWARE LICENSE AGREEMENT:
* Microchip Technology Incorporated ("Microchip") retains all ownership and 
* intellectual property rights in the code accompanying this message and in all 
* derivatives hereto.  You may use this code, and any derivatives created by 
* any person or entity by or on your behalf, exclusively with Microchip's
* proprietary products.  Your acceptance and/or use of this code constitutes 
* agreement to the terms and conditions of this notice.
*
* CODE ACCOMPANYING THIS MESSAGE IS SUPPLIED BY MICROCHIP "AS IS".  NO 
* WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED 
* TO, IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY AND FITNESS FOR A 
* PARTICULAR PURPOSE APPLY TO THIS CODE, ITS INTERACTION WITH MICROCHIP'S 
* PRODUCTS, COMBINATION WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION. 
*
* YOU ACKNOWLEDGE AND AGREE THAT, IN NO EVENT, SHALL MICROCHIP BE LIABLE, WHETHER 
* IN CONTRACT, WARRANTY, TORT (INCLUDING NEGLIGENCE OR BREACH OF STATUTORY DUTY), 
* STRICT LIABILITY, INDEMNITY, CONTRIBUTION, OR OTHERWISE, FOR ANY INDIRECT, SPECIAL, 
* PUNITIVE, EXEMPLARY, INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, FOR COST OR EXPENSE OF 
* ANY KIND WHATSOEVER RELATED TO THE CODE, HOWSOEVER CAUSED, EVEN IF MICROCHIP HAS BEEN 
* ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE.  TO THE FULLEST EXTENT 
* ALLOWABLE BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY RELATED TO 
* THIS CODE, SHALL NOT EXCEED THE PRICE YOU PAID DIRECTLY TO MICROCHIP SPECIFICALLY TO 
* HAVE THIS CODE DEVELOPED.
*
* You agree that you are solely responsible for testing the code and 
* determining its suitability.  Microchip has no obligation to modify, test, 
* certify, or support the code.
*
******************************************************************************/

/******************************************************************************/
/******************************************************************************/
/*                                                                            */
/*                    include files                                           */
/*                                                                            */
/******************************************************************************/
/******************************************************************************/
#define INITIALIZE
#include "general.h"   /* includes types definitions used */

#include "userparms.h" /* contains the motor's params & other calibrations */
#include "periph.h" /* peripherals module - drivers aso */

#include "readadc.h" /* adc read functions */
#include "meascurr.h" /* current read functions */
#include "svgen.h" /* Space Vector Modulation low level routines */


#include "control.h" /* control structure */

#include "pi.h" /* PI controller definitions */
#include "park.h" /* Park transform definitions */

#include "estim.h" /* estimator definitions */
#include "fdweak.h" /* flux weakening routines */


#include "debug.h" /* debug functionality */

#include <libq.h> /* q15 sqrt function use */
#include "RTDM.h"

/******************************************************************************/
/******************************************************************************/
/*                                                                            */
/*                    typedef definitions                                     */
/*                                                                            */
/******************************************************************************/
/******************************************************************************/
union   {
        struct
            {
            unsigned RunMotor:1;  /* run motor indication */
            unsigned OpenLoop:1;  /* open loop/clsd loop indication */
            unsigned Btn1Pressed:1; /* btn 1 pressed indication */
            unsigned Btn2Pressed:1; /* btn 2 pressed indication */
            unsigned ChangeMode:1; /* mode changed indication - from open to clsd loop */
            unsigned ChangeSpeed:1; /* speed doubled indication */
            unsigned    :10;
            }bit;
        WORD Word;
        } uGF;        // general flags


tPIParm     PIParmQ;        /* parms for PI controlers */
tPIParm     PIParmD;        /* parms for PI controlers */
tPIParm     PIParmQref;     /* parms for PI controlers */

tParkParm   ParkParm;       /* park transform params */
tMeasCurrParm MeasCurrParm; /* current measurement params */

tReadADCParm ReadADCParm;   /* adc values structure */

tSVGenParm SVGenParm;       /* SVM structure */

/******************************************************************************/
/******************************************************************************/
/*                                                                            */
/*                    local variables                                         */
/*                                                                            */
/******************************************************************************/
/******************************************************************************/

static unsigned long Startup_Ramp = 0; /* ramp angle variable for initial ramp */
static unsigned int Startup_Lock = 0; /* lock variable for initial ramp */       
static WORD iDispLoopCnt; /* display loop count */


/******************************************************************************/
/******************************************************************************/
/*                                                                            */
/*                    local definitions                                       */
/*                                                                            */
/******************************************************************************/
/******************************************************************************/
/* maximum motor speed converted into electrical speed */
#define MAXIMUMSPEED_ELECTR MAXIMUM_SPEED_RPM*NOPOLESPAIRS
/* nominal motor speed converted into electrical speed */
#define NOMINALSPEED_ELECTR NOMINAL_SPEED_RPM*NOPOLESPAIRS

/* end speed conveted to fit the startup ramp */
#define END_SPEED (END_SPEED_RPM * NOPOLESPAIRS * LOOPTIME_SEC * 65536 / 60.0)*1024
/* end speed of open loop ramp up converted into electrical speed */
#define ENDSPEED_ELECTR END_SPEED_RPM*NOPOLESPAIRS

// Number of control loops that must execute before the button 
// routine is executed.
#define	DISPLOOPTIME	(DISPLOOPTIME_SEC/LOOPTIME_SEC)

/* buttons definitions for the current setup */
#define PINBUTTON1           !PORTGbits.RG7
#define PINBUTTON2           !PORTGbits.RG6

/* maximum pot value in sfrac mode 0xFFC0 */
/* shifted left with 1 meaning 16368 */ 
#define  MAXPOTVAL_SHL1 16368



/******************************************************************************/
/******************************************************************************/
/*                                                                            */
/*                    local functions prototypes                              */
/*                                                                            */
/******************************************************************************/
/******************************************************************************/

void __attribute__((__interrupt__)) _AD1Interrupt(void);
static void InitControlParameters(void);
static void DoControl( void );
static void CalculateParkAngle(void);


/******************************************************************************/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/******************************************************************************/

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/******************************************************************************/
/* Function name: main function                                               */
/* Function parameters: None                                                  */
/* Function return: None                                                      */
/* Description: program entry point, calls the system init function group     */
/* containing the buttons polling loop                                        */
/******************************************************************************/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

int main ( void )
{
    /* init the peripherals - contain the definitions for config bits */
    /* configure PLL, GPIO, PWM & ADC */
    InitPeriph();
    
     /* init PI control parameters */
   	InitControlParameters(); 
   	
   	/* init estim parameters */
   	InitEstimParm();
   	
   	/* init flux weakening params */
   	InitFWParams();

    /*debug initiation - start services */
    DBG_Init();
  
    while(1)
        {
        uGF.Word = 0;                   // clear flags

#ifdef DMCI_DEMO
		DMCIFlags.StartStop = 0;
#endif
        /* setup to openloop */
        uGF.bit.OpenLoop = 1;
        
        /* init PI control parameters */
   	    InitControlParameters();        
        /* init estim parameters */
        InitEstimParm();
   	    /* init flux weakening params */
   	    InitFWParams();
       
        /* Inital offsets for current measurment */
        InitMeasCompCurr( ADC1BUF2, ADC1BUF3); 
        
        // zero out i sums 
        PIParmD.qdSum = 0;
        PIParmQ.qdSum = 0;
        PIParmQref.qdSum = 0;

        /* re set peripheral settings if the case */
        /* duty for PWM, ADC interrupt status */
        ResetPeriph();
		
        if(!uGF.bit.RunMotor)
            {	            
			#ifdef DMCI_DEMO
	            while(!DMCIFlags.StartStop)	// wait here until user starts motor with DMCI
				{
					RTDM_ProcessMsgs();
				}
			#else	            // Initialize current offset compensation
	            while(!PINBUTTON1)                  //wait here until button 1 is pressed 
                {
                        #ifdef RTDM
                            RTDM_ProcessMsgs();	//RTDM process incoming and outgoing messages
			#endif
                }
	            while(PINBUTTON1);                  //when button 1 is released 
		#endif
            /* re set peripherals if necessary */
			ResetPeriph();

            // Inital offsets
        	InitMeasCompCurr( ADC1BUF2, ADC1BUF3);  
            uGF.bit.RunMotor = 1;               //then start motor
            }

        // Run the motor
        uGF.bit.ChangeMode = 1;


		//Run Motor loop
        while(1)
        {
			#ifdef RTDM
		      	RTDM_ProcessMsgs();	//RTDM process incoming and outgoing messages
			#endif                          
	        /* as long as display loop time is not reached */
            /* skip */
            if(iDispLoopCnt >= DISPLOOPTIME)
            	{
				if (uGF.bit.RunMotor == 0)
				/* exit loop if motor not runned */
					break;

				#ifdef DMCI_DEMO
					if(!DMCIFlags.StartStop)
					{	
                         /* set the reference speed value to 0 */
                         CtrlParm.qVelRef = 0;
                         /* restart in open loop */
                         uGF.bit.OpenLoop = 1;
                         // Button just released
                         uGF.bit.Btn1Pressed  = 0;
                         // Button just released (forced)
                         uGF.bit.Btn2Pressed  = 0;
                         // change speed 
                         uGF.bit.ChangeSpeed =0;
                         // change mode 
                         uGF.bit.ChangeMode =0;
                         // begin stop sequence
                         uGF.bit.RunMotor = 0;
                         // reset periph
                         ResetPeriph();
                         break;
					}
				#else
	      			// Button 1 starts or stops the motor
	                if(PINBUTTON1)  
		                {
	                    if( !uGF.bit.Btn1Pressed )
	                        {
	                            uGF.bit.Btn1Pressed  = 1;
	                        }
	                    }
	                else
	                    {
	                    if( uGF.bit.Btn1Pressed )
	                        {
	                            /* set the reference speed value to 0 */
	                            CtrlParm.qVelRef = 0;
	                            /* restart in open loop */
	                            uGF.bit.OpenLoop = 1;
	                            // Button just released
	                            uGF.bit.Btn1Pressed  = 0;
	                            // Button just released (forced)
	                            uGF.bit.Btn2Pressed  = 0;
	                            // change speed 
	                            uGF.bit.ChangeSpeed =0;
	                            // change mode 
	                            uGF.bit.ChangeMode =0;
	                            // begin stop sequence
	                            uGF.bit.RunMotor = 0;
	                            // reset periph
	                            ResetPeriph();
	                            break;
	                        }
	                    }
	                    
					
	                if(PINBUTTON2)                      
	                    {
	                    if( !uGF.bit.Btn2Pressed )
	                        uGF.bit.Btn2Pressed  = 1;
	                        /*start the debug snap */
	                        /* the data will be captured from this point */
	                        DBG_SnapStart();
	                    }
	                else
	                    {
	                    if( uGF.bit.Btn2Pressed )
	                        {
	                        // Button just releasedk
	                        uGF.bit.Btn2Pressed  = 0;
	                        /* switch back the change speed indication */
	                        uGF.bit.ChangeSpeed = !uGF.bit.ChangeSpeed;
	                        }
	                    }
				#endif				

                }  // end of display and button polling code              
                
            }   // End of Run Motor loop

        
        } // End of Main loop
        
    // should never get here
    while(1){}
}

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/******************************************************************************/
/* Function name: DoControl                                                   */
/* Function parameters: None                                                  */
/* Function return: None                                                      */
/* Description:                                                               */
/* Executes one PI itteration for each of the three loops Id,Iq,Speed         */
/******************************************************************************/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

static void DoControl( void )
{
    /* temporary vars for sqrt calculation of q reference */
    int temp_qref_pow_q15;

#ifdef TUNING
    static int tuning_add_rampup = 0; // tuning speed ramp value 
    static int tuning_delay_rampup;   // tuning speed ramp increase delay
#endif

    if( uGF.bit.OpenLoop )
    {
        // OPENLOOP:  force rotating angle,Vd,Vq
        if( uGF.bit.ChangeMode )
        {
            // just changed to openloop
            uGF.bit.ChangeMode = 0;
            // synchronize angles

            // VqRef & VdRef not used
            CtrlParm.qVqRef = 0;
            CtrlParm.qVdRef = 0;

			/* reinit vars for initial speed ramp */
			Startup_Lock = 0;
			Startup_Ramp = 0;
#ifdef TUNING
            tuning_add_rampup = 0;
#endif
        }
        
        /* speed reference */
		CtrlParm.qVelRef = Q_CURRENT_REF_OPENLOOP;
        /* q current reference is equal to the vel reference */
        /* while d current reference is equal to 0 */
        /* for maximum startup torque, set the q current to maximum acceptable */
        /* value represents the maximum peak value */
        CtrlParm.qVqRef    = CtrlParm.qVelRef;
       	
        // 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;


    } else
    // Closed Loop Vector Control
	{
        /* if change speed indication, double the speed */
        if(uGF.bit.ChangeSpeed)
	    {
/* if Bidirectional functioning, disable speed doubling */
#ifndef BIDIRECTIONAL_SPEED
		#ifdef DMCI_DEMO
			ReadADCParm.qADValue = SpeedReference;
		#else
		    /* read not signed ADC */
		    ReadADC0( &ReadADCParm );
		#endif
                     ReadADCParm.qAnRef=(ReadADCParm.qADValue);	
                     if(ReadADCParm.qAnRef >MAXIMUMSPEED_ELECTR)ReadADCParm.qAnRef = MAXIMUMSPEED_ELECTR;

#else
		#ifdef DMCI_DEMO
			ReadADCParm.qADValue = SpeedReference;
		#else
		    /* unsigned values */
		    ReadSignedADC0( &ReadADCParm );
		#endif
		    /* ADC values are shifted with 2 */
		    ReadADCParm.qAnRef=ReadADCParm.qADValue>>2;		// Speed pot ref max value +-8190
#endif		    
	    } else
	    {
/* if Bidirectional, read pot value signed */
#ifdef BIDIRECTIONAL_SPEED
		#ifdef DMCI_DEMO
			ReadADCParm.qADValue = SpeedReference;
		#else
		    /* signed values, different than in case of FW above */
		    ReadSignedADC0( &ReadADCParm );
		#endif
#else
		#ifdef DMCI_DEMO
			ReadADCParm.qADValue = SpeedReference;
		#else
		    /* unsigned values */
		    ReadADC0( &ReadADCParm );
		#endif
#endif
		    /* ADC values are shifted with 2 */
		    ReadADCParm.qAnRef=ReadADCParm.qADValue>>2;		// Speed pot ref max value +-8190
	    }

        // Ramp generator to limit the change of the speed reference
        // the rate of change is defined by CtrlParm.qRefRamp
    	
    	CtrlParm.qDiff=CtrlParm.qVelRef - ReadADCParm.qAnRef;		
        // Speed Ref Ramp
		if (CtrlParm.qDiff < 0)
		{
			/* set this cycle reference as the sum of */
			/* previously calculated one plus the reframp value */
			CtrlParm.qVelRef=CtrlParm.qVelRef+CtrlParm.qRefRamp;
		}
		else
		{
			/* same as above for speed decrease */
			CtrlParm.qVelRef=CtrlParm.qVelRef-CtrlParm.qRefRamp;
		}
		/* if difference less than half of ref ramp, set reference */
		/* directly from the pot */
		if (_Q15abs(CtrlParm.qDiff) < (CtrlParm.qRefRamp<<1))
		{
	    	CtrlParm.qVelRef = ReadADCParm.qAnRef;
		}

        /* tuning is generating a software ramp */
        /* with sufficiently slow ramp defined by */
        /* TUNING_DELAY_RAMPUP constant */
#ifdef TUNING
        /* if delay is not completed */
        if(tuning_delay_rampup > TUNING_DELAY_RAMPUP)tuning_delay_rampup = 0;
        /* while speed less than maximum and delay is complete */
        if((tuning_add_rampup < (MAXIMUMSPEED_ELECTR - ENDSPEED_ELECTR)) && (tuning_delay_rampup == 0) ) 
        {
            /* increment ramp add */
            tuning_add_rampup++;
        }
        tuning_delay_rampup++;
        /* the reference is continued from the open loop speed up ramp */
		CtrlParm.qVelRef =   ENDSPEED_ELECTR +  tuning_add_rampup;
#endif
	    
        if( uGF.bit.ChangeMode )
        {
            // just changed from openloop
            uGF.bit.ChangeMode = 0;
			PIParmQref.qdSum = (long)CtrlParm.qVqRef << 13;

	    }               

/* if TORQUE MODE skip the speed controller */                
#ifndef	TORQUE_MODE
       	// Execute the velocity control loop
		PIParmQref.qInMeas = EstimParm.qVelEstim;
    	PIParmQref.qInRef  = CtrlParm.qVelRef;
    	CalcPI(&PIParmQref);
    	CtrlParm.qVqRef = PIParmQref.qOut;

#else
        CtrlParm.qVqRef = CtrlParm.qVelRef;
#endif

        /* Snap start for SNAPSHOT */  
        DBG_SnapStart();

        /* snap vars updated for both SNAPSHOT and RTDM */
        DBG_SnapUpdate();
        

        
        /* Flux weakenign control - the actual speed is replaced */
        /* with the reference speed for stability */
        /* reference for d curent component */
        /* adapt the estimator parameters in concordance with the speed */
        CtrlParm.qVdRef=FieldWeakening(_Q15abs(CtrlParm.qVelRef));
        
        // PI control for D
        PIParmD.qInMeas = ParkParm.qId;
        PIParmD.qInRef  = CtrlParm.qVdRef;
        CalcPI(&PIParmD);
        ParkParm.qVd    = PIParmD.qOut;

        /* dynamic d-q adjustment */
        /* with d component priority */
        /* vq=sqrt (vs^2 - vd^2) */
        /* limit vq maximum to the one resulting from the calculation above */
        temp_qref_pow_q15 = (int)(__builtin_mulss(PIParmD.qOut , PIParmD.qOut)>>15);
        temp_qref_pow_q15 = Q15(0.98) - temp_qref_pow_q15;
        PIParmQ.qOutMax = Q15SQRT (temp_qref_pow_q15);
 
        // PI control for Q
        PIParmQ.qInMeas = ParkParm.qIq;
        PIParmQ.qInRef  = CtrlParm.qVqRef;
        CalcPI(&PIParmQ);
        ParkParm.qVq    = PIParmQ.qOut;       
    }
}

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/******************************************************************************/
/* Function name: ADC1Interrupt                                               */
/* Function parameters: None                                                  */
/* Function return: None                                                      */
/* Description: 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, no_auto_psv)) _AD1Interrupt(void)
{
	IFS0bits.AD1IF = 0; 
    
    // pin toggle for debug
    // PORTAbits.RA2 = 1;
    
    // Increment count variable that controls execution
    // of display and button functions.
    iDispLoopCnt++;
             
    if( uGF.bit.RunMotor )
    {
	    // Calculate qIa,qIb
        MeasCompCurr();
        
        // Calculate qId,qIq from qSin,qCos,qIa,qIb
        ClarkePark();
  
		        
        // Speed and field angle estimation
        //****************************            
        Estim();
        
        // Calculate control values
        DoControl();
        
        // Calculate qAngle from QEI Module
		CalculateParkAngle();
        
        /* if open loop */
		if(uGF.bit.OpenLoop == 1)
		{
		    /* the angle is given by parkparm */
		    SincosParm.qAngle = ParkParm.qAngle;
		} else
		{
			/* if closed loop, angle generated by estim */
			SincosParm.qAngle = EstimParm.qRho;
        }
        	
	    // Calculate qSin,qCos from qAngle
        SinCos();
    
        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();

    } 
    // pin toggle for debug
    // PORTAbits.RA2 = 0;
}

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/******************************************************************************/
/* Function name: CalculateParkAngle                                          */
/* Function parameters: None                                                  */
/* Function return: None                                                      */
/* Description: Generate the start sinwaves feeding the motor's terminals     */
/* Open loop control, forcing the motor to allign and to start speeding up    */
/******************************************************************************/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static void CalculateParkAngle(void)
{
    /* if open loop */
	if(uGF.bit.OpenLoop)	
	{
		/* begin wiht the lock sequence, for field alligniament */
		if (Startup_Lock < LOCK_TIME)
			Startup_Lock+=1;
	    /* then ramp up till the end speed */
		else if (Startup_Ramp < END_SPEED)
			Startup_Ramp+=OPENLOOP_RAMPSPEED_INCREASERATE;
		else /* switch to closed loop */
		{
#ifndef OPEN_LOOP_FUNCTIONING
            uGF.bit.ChangeMode = 1;
            uGF.bit.OpenLoop = 0;
#endif
		}
		/* the angle set depends on startup ramp */
		ParkParm.qAngle += (int)(Startup_Ramp >> 10);
	}
	else /* switched to closed loop */
	{
   	    /* in closed loop slowly decrease the offset add to */
   	    /* the estimated angle */
   	    if(EstimParm.RhoOffset>0)EstimParm.RhoOffset--; 
	}
	return;
}

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/******************************************************************************/
/* Function name: InitControlParameters                                       */
/* Function parameters: None                                                  */
/* Function return: None                                                      */
/* Description:                                                               */
/* Init control parameters: PI coefficients, scalling consts etc.             */
/******************************************************************************/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static void InitControlParameters(void)
{

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

    // Scaling constants: Determined by calibration or hardware design.
    ReadADCParm.qK      = KPOT;    
    MeasCurrParm.qKa    = KCURRA;    
    MeasCurrParm.qKb    = KCURRB;   
    
    CtrlParm.qRefRamp = SPEEDREFRAMP;
    
    // ============= SVGen ===============
    // Set PWM period to Loop Time 
    SVGenParm.iPWMPeriod = LOOPTIME_TCY;
    
    // ============= PI D Term ===============      
    PIParmD.qKp = D_CURRCNTR_PTERM;       
    PIParmD.qKi = D_CURRCNTR_ITERM;              
    PIParmD.qKc = D_CURRCNTR_CTERM;       
    PIParmD.qOutMax = D_CURRCNTR_OUTMAX;
    PIParmD.qOutMin = -PIParmD.qOutMax;

    InitPI(&PIParmD);

    // ============= PI Q Term ===============
    PIParmQ.qKp = Q_CURRCNTR_PTERM;    
    PIParmQ.qKi = Q_CURRCNTR_ITERM;
    PIParmQ.qKc = Q_CURRCNTR_CTERM;
    PIParmQ.qOutMax = Q_CURRCNTR_OUTMAX;
    PIParmQ.qOutMin = -PIParmQ.qOutMax;

    InitPI(&PIParmQ);

    // ============= PI Qref Term ===============
    PIParmQref.qKp = SPEEDCNTR_PTERM;       
    PIParmQref.qKi = SPEEDCNTR_ITERM;       
    PIParmQref.qKc = SPEEDCNTR_CTERM;       
    PIParmQref.qOutMax = SPEEDCNTR_OUTMAX;   
    PIParmQref.qOutMin = -PIParmQref.qOutMax;

    InitPI(&PIParmQref);
	return;
}


