/*******************************************************************************
Copyright (c) 2014 released Microchip Technology Inc. All rights reserved.

Microchip licenses to you the right to use, modify, copy and distribute
Software only when embedded on a Microchip microcontroller or digital signal
controller that is integrated into your product or third party product
(pursuant to the sublicense terms in the accompanying license agreement).

You should refer to the license agreement accompanying this Software for
additional information regarding your rights and obligations.

SOFTWARE AND DOCUMENTATION ARE PROVIDED AS IS WITHOUT WARRANTY OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF
MERCHANTABILITY, TITLE, NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE.
IN NO EVENT SHALL MICROCHIP OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER
CONTRACT, NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR
OTHER LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE OR
CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT OF
SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
(INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
*******************************************************************************/
/************** GLOBAL DEFINITIONS ***********/

#ifdef __XC16__  // See comments at the top of this header file
    #include <xc.h>
#endif // __XC16__
#define INITIALIZE

#include "general.h"
#include "parms.h"
#include "read_adc.h"
#include "meas_curr.h"
#include "control.h"
#include "user_parms.h"
#include "smcpos.h"
#include "field_weak.h"
#include "RTDM.h"
#include <uart.h>
#include "mcp802x_de2.h"
#include <stdint.h>
#include <libq.h>
#include "motor_control.h"
#include "periph.h"



/********************* Variables to display data using DMCI *********************************/

#ifdef RTDM

int RecorderBuffer1[DATA_BUFFER_SIZE]; /*Buffer to store the data samples for the DMCI data viewer Graph1 */
int RecorderBuffer2[DATA_BUFFER_SIZE]; /*Buffer to store the data samples for the DMCI data viewer Graph2 */
int RecorderBuffer3[DATA_BUFFER_SIZE]; /*Buffer to store the data samples for the DMCI data viewer Graph3 */
int RecorderBuffer4[DATA_BUFFER_SIZE]; /*Buffer to store the data samples for the DMCI data viewer Graph4 */

int * PtrRecBuffer1 = &RecorderBuffer1[0]; /*Tail pointer for the DMCI Graph1 */
int * PtrRecBuffer2 = &RecorderBuffer2[0]; /*Tail pointer for the DMCI Graph2 */
int * PtrRecBuffer3 = &RecorderBuffer3[0]; /*Tail pointer for the DMCI Graph3 */
int * PtrRecBuffer4 = &RecorderBuffer4[0]; /*Tail pointer for the DMCI Graph4 */
int * RecBuffUpperLimit = RecorderBuffer4 + DATA_BUFFER_SIZE - 1; /*Buffer Recorder Upper Limit */

typedef struct DMCIFlags
{
    unsigned Recorder : 1; /* Flag needs to be set to start buffering data */
    unsigned StartStop : 1;
    unsigned unused : 14;
} DMCIFLAGS;
DMCIFLAGS DMCIFlags;
int SnapCount = 0;

int SnapShotDelayCnt = 0;
int SnapShotDelay = SNAPDELAY;
int SpeedReference = 0;

#endif /* End of #ifdef RTDM */

MC_PIPARMIN_T tPIParmInIQ;  /* Structure definition for Torque component of current, or Iq */
MC_PIPARMIN_T tPIParmInID; /* Structure definition for Flux component of current, or Id */
MC_PIPARMIN_T tPIParmInWA; /* Structure definition for Speed, or Omega */
MC_PIPARMOUT_T tPIParmOutIQ; /* Structure definition for Torque component of current, or Iq */
MC_PIPARMOUT_T tPIParmOutID; /* Structure definition for Flux component of current, or Id */
MC_PIPARMOUT_T tPIParmOutWA; /* Structure definition for Speed, or Omega */
MC_SINCOS_T tSinCosTheta; /* Structure definition for sinecosine of angle */
MC_DUTYCYCLEOUT_T tPWMDutyValue; /* Structure definition for PWM duty cycle */
MC_DQ_T tParkVoltage,tParkCurrent; /* Structure definition for Park Voltage and Park Current*/
MC_ALPHABETA_T tClarkVoltage,tClarkCurrent; /* Structure definition for Clark Voltage and Clark Current*/
MC_ABC_T tPhaseVoltages,tPhaseCurrents; /* Structure definition for Phase Voltages and Phase Current*/

int16_t qAngle = 0;
int16_t count = 0; /* delay for ramping the reference velocity */
int16_t velReq = 0;

SMC smc1 = SMC_DEFAULTS;
ADC_OFFSET_T adcOffset;
uint32_t startupRamp = 0;   /* Start up ramp in open loop. This variable
                            is incremented in CalculateParkAngle()
                            subroutine, and it is assigned to
                            qAngle as follows:
                            qAngle += (int)(startupRamp >> 16);*/

uint16_t startupLock = 0; /* This is a counter that is incremented in
								CalculateParkAngle() every time it is called. 
								Once this counter has a value of LOCK_TIME, 
								then theta will start increasing moving the 
								motor in open loop. */
uint16_t transCounter = 0;

union
{

    struct
    {
        unsigned openLoop : 1; /* Indicates if motor is running in open or closed loop */
        unsigned runMotor : 1; /* If motor is running, or stopped.*/
        unsigned enTorqueMod : 1; /* This bit enables Torque mode when running closed loop */
        unsigned enVoltRipCo : 1; /* Bit that enables Voltage Ripple Compensation */
        unsigned btn1Pressed : 1; /* Button 1 has been pressed. */
        unsigned btn2Pressed : 1; /* Button 2 has been pressed. */
        unsigned changeMode : 1; 
        /* This flag indicates that a transition from open to closed */
        /* loop, or closed to open loop has happened. This
        /* causes DoControl subroutine to initialize some variables
        /* before executing open or closed loop for the first time */
        unsigned changeSpeed : 1; 
        /* This flag indicates a step command in speed reference.
        /* This is mainly used to analyze step response */
        unsigned adch0PotOrVdc : 1; /*This bit Indicates ADC Channel O Sample A or Sample B is converted */
        unsigned stop : 1;     /*this bit is to indicate motor has stop*/
        unsigned : 6;
    } bits;
    uint16_t word;
} uGF;


MEAS_CURR_PARM_T measCurrParm;         /* current measurement params */
READADC_PARM_T tReadADCParm_A;      /* Struct used to read ADC values. */
/* Speed Calculation Variables */

uint16_t iADCisrCnt = 0; /* This Counter is used as a timeout for polling the push buttons */
/* in main() subroutine. It will be reset to zero when it matches */
/* dButPolLoopCnt defined in UserParms.h*/
int16_t prevTheta = 0; /* Previous theta which is then substracted from Theta to get */
/* delta theta. This delta will be accumulated in accumTheta, and */
/* after a number of accumulations Omega is calculated. */
int16_t accumTheta = 0; /* Accumulates delta theta over a number of times */
uint16_t accumThetaCnt = 0; /* Counter used to calculate motor speed. Is incremented */
/* in SMC_Position_Estimation() subroutine, and accumulates */
/* delta Theta. After N number of accumulations, Omega is */
/* calculated. This N is diIrpPerCalc which is defined in */
/* UserParms.h. */

/* Vd and Vq vector limitation variables */

int16_t qVdSquared = 0; /* This variable is used to know what is left from the VqVd vector */
/* in order to have maximum output PWM without saturation. This is */
/* done before executing Iq control loop at the end of DoControl() */

int16_t dcBus = 0; /* DC Bus measured continuously and stored in this variable
/* while motor is running. Will be compared with targetDcBus
/* and Vd and Vq will be compensated depending on difference
/* between dcBus and targetDcBus */

int16_t targetDcBus = 0; /* DC Bus is measured before running motor and stored in this */
/* variable. Any variation on DC bus will be compared to this value */
/* and compensated linearly.	*/

int16_t thetaError = 0; /* This value is used to transition from open loop to closed looop.
/* At the end of open loop ramp, there is a difference between
/* forced angle and estimated angle. This difference is stored in
/* thetaError, and added to estimated theta (smc1.Theta) so the
/* effective angle used for commutating the motor is the same at
/* the end of open loop, and at the begining of closed loop.
/* This thetaError is then substracted from estimated theta
/* gradually in increments of 0.05 degrees until the error is less
/* than 0.05 degrees. */

uint16_t iPwmPeriod;

typedef union
{

    struct
    {
        unsigned Status : 2;
        unsigned Value : 1;
        unsigned : 5;
        unsigned char Debounce_Count;
    } member;
    uint16_t Word;
} T_BUTTON;
T_BUTTON buttonStartStopA, buttonHalfDoubleA;
void ResetParms(void);
void ButtonScan(T_BUTTON *);
void InitPeripherals (void);

/************* START OF MAIN FUNCTION ***************/
int main(void)
{
    InitOscillator();
    SetupGpioPorts();
    SMCInit(&smc1);
    SetupControlParameters();
    FWInit();
    uGF.word = 0; /* clear flags */
    CE_A_MCP802x = 0; /* Setting CE_A = 0 */
#ifdef RTDM
    RTDM_Start(); /* Configure the UART module used by RTDM */
    /* it also initializes the RTDM variables */
#endif
#ifdef TORQUEMODE
    uGF.bits.enTorqueMod = 1;
#endif

#ifdef ENVOLTRIPPLE
    uGF.bits.enVoltRipCo = 1;
    uGF.bits.adch0PotOrVdc = 0; /* Selects POT as CHO Input */
#endif


#ifdef RTDM
    DMCIFlags.StartStop = 0;
    DMCIFlags.Recorder = 0;
#endif
    CE_A_MCP802x = 1; /* Setting CE_A  = 1 */
    ConfigureMcp8024(DE2_UART_MCP8024_A );
    /* Initialise peripherals PWMS,ADC,Op-AMP/COmparators used
       for Inverter A control */
    InitPeripherals();



    while (1)
    {
        ResetParms();

        if (!uGF.bits.runMotor)
        {
        #ifdef DMCI_DEMO

            while (!DMCIFlags.StartStop) /* wait here until user starts motor */
            { /* with DMCI */
              ResetParms();
              uGF.bits.runMotor = 1; /*then start motor */
        #ifdef RTDM
                RTDM_ProcessMsgs();
        #endif
      
        }
        #else
        
        #endif
        }
        /* Run the motor */

        while (1)
        {
        if (uGF.bits.stop)
           {
              ResetParms();  /*reset parameters if motor stalls*/
           }

        #ifdef RTDM
            RTDM_ProcessMsgs(); /*RTDM process incoming and outgoing messages */
        #endif

           /* The code that polls the buttons executes every 100 msec. */
            if (iADCisrCnt >= BUTPOLLOOPCNT)
            {
 

        #ifdef DMCI_DEMO
                if (!DMCIFlags.StartStop)
                {
                    uGF.bits.runMotor = 0;
                    transCounter = 0; /*initialize transition counter */
                    break;
                }
        #else
                /* Button scanning loop for Button 1 to start/stop Motor A */
                buttonStartStopA.member.Value = BTN_START_STOP_A;
                ButtonScan(&buttonStartStopA);
                if (buttonStartStopA.member.Status == BTN_PRESSED)
                {
                    buttonStartStopA.member.Status = BTN_NOT_PRESSED;
                    if (uGF.bits.runMotor == 1) /* Toggle RunMotor flag for motor1 */
                    {
                        /* Stop Motor and reset parameters and PWM duty cyle*/
                        ResetParms();
                    }
                    else
                    {
                         /* Start Motor*/

                        uGF.bits.runMotor = 1;
                    }

                }
                if (uGF.bits.runMotor == 1)
                {
                   /* To verify the DE2 status and take action */
                    StatusCheckMcp802x(DE2_UART_MCP8024_A );
                    /* Button scanning loop for Button 2 to enter into filed weakening mode */
                    buttonHalfDoubleA.member.Value = BTN_SPD_HALF_DBL_A;
                    ButtonScan(&buttonHalfDoubleA);
                    if (buttonHalfDoubleA.member.Status == BTN_PRESSED)
                    {
                        buttonHalfDoubleA.member.Status = BTN_NOT_PRESSED;
                        /* Indicates change in mode */
                        uGF.bits.changeSpeed = !uGF.bits.changeSpeed;

                    }
                }
           
                
          
                /* Button 1 starts or stops the motor */

#endif

                /*while running button 2 will double/half the speed */

            } /* end of button polling code */
        } /* End of Run Motor loop */
     } /* End of Main loop */
        /* should never get here  */
    while (1)
    {
    }
}

/******************************************************************************
 * Function:    ButtonScan()
 *
 * Output:	None
 *
 * Overview:	Routine to scan the push buttons
 *
 *******************************************************************************/
void ButtonScan(T_BUTTON *Button)
{
    if (Button->member.Value == 0)
    {
        if (Button->member.Debounce_Count < BTN_DEBOUNCE_COUNT )
        {
            Button->member.Debounce_Count++;
            Button->member.Status = BTN_DEBOUNCE;
        }
    }
    else
    {
        if (Button->member.Debounce_Count < BTN_DEBOUNCE_COUNT )
        {
            Button->member.Status = BTN_NOT_PRESSED;
        }
        else
        {
            Button->member.Status = BTN_PRESSED;
        }
        Button->member.Debounce_Count = 0;
    }
}

/******************************************************************************
 * Function:    Reset_Parms()
 *
 * Output:	None
 *
 * Overview:	Routine to reset Inverter A parameters and PWMs etc
 *
 *******************************************************************************/
void ResetParms(void)
{
     uGF.bits.stop = 0;
    uGF.bits.runMotor = 0; /* Stop the motor
    /* low side turn on errate workaraund */
    INVERTERA_PWM_PDC1 = MIN_DUTY; /* PDC cannot be init with 0, please check errata */
    INVERTERA_PWM_PDC2 = MIN_DUTY;
    INVERTERA_PWM_PDC3 = MIN_DUTY;
    DisableAdc1Interrupt(); /* Make sure ADC does not generate */
    /* interrupts while parameters */
    buttonStartStopA.Word = 0;
    buttonStartStopA.Word = 0;
    uGF.bits.changeSpeed = 0;
    /* This causes DoControl subroutine to initialize some variables
    before executing open or closed loop for the first time */
   uGF.bits.changeMode = 1;
    /* init Mode - start in openloop */
    uGF.bits.openLoop = 1; /* start in openLoop */

    transCounter = 0; /*initialize transition counter */

    /* zero out i sums */
    tPIParmInID.piState.integrator = 0;
    tPIParmInIQ.piState.integrator = 0;
    tPIParmInWA.piState.integrator = 0;

    /* Enable ADC interrupt and begin main loop timing */
    ClearADC1IF();
    EnableAdc1Interrupt();

}

/*--------------------------------------------------------------------- */
/* Executes one PI itteration for each of the three loops Id,Iq,Speed, */

inline void DoControl(void)
{

#ifndef	DMCI_DEMO
#ifndef ENVOLTRIPPLE
    ReadSignedADC0(ADCBUF_SPEED_REF_A,&tReadADCParm_A);
#endif
#endif

#ifdef ENVOLTRIPPLE
    if (uGF.bits.adch0PotOrVdc == 0)
    {
        ReadSignedADC0(ADCBUF_SPEED_REF_A,&tReadADCParm_A);
        AD1CHS0bits.CH0SA = 10; /* Selects DCBUS as CHO Input for next conversion */
        uGF.bits.adch0PotOrVdc = 1;
    }
    else if (uGF.bits.adch0PotOrVdc == 1)
    {
        ReadAdc0V_DC(ADCBUF_VBUS_A, &dcBus);
        AD1CHS0bits.CH0SA = 13; /* Selects POT as CHO Input for next conversion */
        uGF.bits.adch0PotOrVdc = 0;
    }
#endif


    if (uGF.bits.openLoop)
    {
        /* openLoop:	force rotating angle, and control Iq and Id
        /*				Also limits Vs vector to ensure maximum PWM duty
        /*				cycle and no saturation

        /* This If statement is executed only the first time we enter open loop, */
        /* everytime we run the motor */
        if (uGF.bits.changeMode)
        {
            /* just changed to openLoop */
            uGF.bits.changeMode = 0;
            /* synchronize angles */

            /* VqRef & VdRef not used */
            ctrlParm.qVqRef = 0;
            ctrlParm.qVdRef = 0;
            ctrlParm.qVelRef = 0;
            startupLock = 0;
            startupRamp = 0;
            /* Initialize SMC */
            smc1.vAlpha = 0;
            smc1.eAlpha = 0;
            smc1.eAlphaFinal = 0;
            smc1.zAlpha = 0;
            smc1.estIalpha = 0;
            smc1.vBeta = 0;
            smc1.eBeta = 0;
            smc1.eBetaFinal = 0;
            smc1.zBeta = 0;
            smc1.estIbeta = 0;
            smc1.iAlpha = 0;
            smc1.iAlphaError = 0;
            smc1.iBeta = 0;
            smc1.iBetaError = 0;
            smc1.theta = 0;
            smc1.omega = 0;
        }

        /* Enter initial torque demand in Amps using REFINAMPS() macro.
        /* Maximum Value for reference is defined by shunt resistor value and
        /* differential amplifier gain. Use this equation to calculate
        /* maximum torque in Amperes:
        /*
        /* Max REFINAMPS = (VDD/2)/(RSHUNT*DIFFAMPGAIN)
        /*
        /* For example:
        /*
        /* RSHUNT = 0.005
        /* VDD = 3.3
        /* DIFFAMPGAIN = 75
        /*
        /* Maximum torque reference in Amps is:
        /*
        /* (3.3/2)/(.005*75) = 4.4 Amperes, or REFINAMPS(4.4)
        /*
        /* If motor requires more torque than Maximum torque to startup, user
        /* needs to change either shunt resistors installed on the board,
        /* or differential amplifier gain. */

        ctrlParm.qVqRef = REFINAMPS(INITIAL_TORQUE);

        if (accumThetaCnt == 0)
        {
            tPIParmInWA.inMeasure = smc1.omega;
        }

        /* PI control for D */
        tPIParmInID.inMeasure = tParkCurrent.d;
        tPIParmInID.inReference = ctrlParm.qVdRef;
        MC_ControllerPIUpdate_Assembly(tPIParmInID.inReference, tPIParmInID.inMeasure, &tPIParmInID.piState, &tPIParmOutID.out);
        tParkVoltage.d = tPIParmOutID.out;

        /* Vector limitation
        /* Vd is not limited
        /* Vq is limited so the vector Vs is less than a maximum of 95%.
        /* The 5% left is needed to be able to measure current through
        /* shunt resistors.
        /* Vs = SQRT(Vd^2 + Vq^2) < 0.95
        /* Vq = SQRT(0.95^2 - Vd^2) */
        qVdSquared = FracMpy(tPIParmOutID.out, tPIParmOutID.out);
        tPIParmInIQ.piState.outMax = Q15SQRT(Q15(0.95 * 0.95) - qVdSquared);
        tPIParmInIQ.piState.outMin = -tPIParmInIQ.piState.outMax;

        /* PI control for Q */
        tPIParmInIQ.inMeasure = tParkCurrent.q;
        tPIParmInIQ.inReference = ctrlParm.qVqRef;
        MC_ControllerPIUpdate_Assembly(tPIParmInIQ.inReference, tPIParmInIQ.inMeasure, &tPIParmInIQ.piState, &tPIParmOutIQ.out);
        tParkVoltage.q = tPIParmOutIQ.out;

    }
    else
        /* Closed Loop Vector Control */
    {
        if (++count == SPEED_DELAY)
        {
#ifdef DMCI_DEMO
            velReq = FracMpy(SpeedReference, DQK) + Q15((OMEGA10 + OMEGA1) / 2.0);
#else
            velReq = tReadADCParm_A.qADValue + Q15((OMEGA10 + OMEGA1) / 2.0);
#endif


            if ((uGF.bits.changeSpeed) && (ctrlParm.qVelRef <= velReq)) /* 2x speed */
            {
                ctrlParm.qVelRef += SPEED_DELAY;
            }
            else if (ctrlParm.qVelRef <= velReq / 2) /*normal speed */
            {
                ctrlParm.qVelRef += SPEED_DELAY;
            }
            else
            {
                ctrlParm.qVelRef -= SPEED_DELAY;
            }
            count = 0;
        }

        /* When it first transition from open to closed loop, this If statement is
           executed */
        if (uGF.bits.changeMode)
        {
            /* just changed from openLoop */
            uGF.bits.changeMode = 0;
            tPIParmInWA.piState.integrator = (long) ctrlParm.qVqRef << 16;
            startupLock = 0;
            startupRamp = 0;
            /* velocity reference ramp begins at minimum speed */
            ctrlParm.qVelRef = Q15(OMEGA0);

        }

        /* 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 (accumThetaCnt == 0)
        {
            /* Execute the velocity control loop */
            tPIParmInWA.inMeasure = smc1.omega;
            tPIParmInWA.inReference = ctrlParm.qVelRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInWA.inReference, tPIParmInWA.inMeasure, &tPIParmInWA.piState, &tPIParmOutWA.out);
            ctrlParm.qVqRef = tPIParmOutWA.out;
        }

        /* 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. This feature is enabled automatically only if
        /* #define TORQUEMODE is defined in UserParms.h. If this is not
        /* defined, uGF.bit.enTorqueMod bit can be set in debug mode to enable
        /* torque mode as well. */

        if (uGF.bits.enTorqueMod)
        {
            ctrlParm.qVqRef = ctrlParm.qVelRef;
        }
        /* Get Id reference from Field Weakening table. If Field weakening
        /* is not needed or user does not want to enable this feature,
        /* let NOMINALSPEEDINRPM be equal to FIELDWEAKSPEEDRPM in
        /* UserParms.h */
        ctrlParm.qVdRef = FieldWeakening(_Q15abs(ctrlParm.qVelRef));

        /* PI control for D */
        tPIParmInID.inMeasure = tParkCurrent.d;
        tPIParmInID.inReference = ctrlParm.qVdRef;
        MC_ControllerPIUpdate_Assembly(tPIParmInID.inReference, tPIParmInID.inMeasure, &tPIParmInID.piState, &tPIParmOutID.out);


        /* If voltage ripple compensation flag is set, adjust the output
        /* of the D controller depending on measured DC Bus voltage. This
        /* feature is enabled automatically only if #define ENVOLTRIPPLE is
        /* defined in UserParms.h. If this is not defined, uGF.bit.enVoltRipCo
        /* bit can be set in debug mode to enable voltage ripple compensation.
        /*
        /* NOTE:
        /*
        /* If Input power supply has switching frequency noise, for example if a
        /* switch mode power supply is used, Voltage Ripple Compensation is not
        /* recommended, since it will generate spikes on Vd and Vq, which can
        /* potentially make the controllers unstable.*/
        if (uGF.bits.enVoltRipCo)
        {
            tParkVoltage.d = VoltRippleComp(tPIParmOutID.out);
        }
        else
        {
            tParkVoltage.d = tPIParmOutID.out;
        }
        /* Vector limitation
        /* Vd is not limited
        /* Vq is limited so the vector Vs is less than a maximum of 95%.
        /* Vs = SQRT(Vd^2 + Vq^2) < 0.95
        /* Vq = SQRT(0.95^2 - Vd^2)*/

        qVdSquared = FracMpy(tParkVoltage.d, tParkVoltage.d);

        tPIParmInIQ.piState.outMax = Q15SQRT(Q15(0.95 * 0.95) - qVdSquared);
        tPIParmInIQ.piState.outMin = -tPIParmInIQ.piState.outMax;

        /* PI control for Q */
        tPIParmInIQ.inMeasure = tParkCurrent.q;
        tPIParmInIQ.inReference = ctrlParm.qVqRef;
        MC_ControllerPIUpdate_Assembly(tPIParmInIQ.inReference, tPIParmInIQ.inMeasure, &tPIParmInIQ.piState, &tPIParmOutIQ.out);


        /* If voltage ripple compensation flag is set, adjust the output
        /* of the Q controller depending on measured DC Bus voltage */
        if (uGF.bits.enVoltRipCo)
        {
            tParkVoltage.q = VoltRippleComp(tPIParmOutIQ.out);
        }
        else
        {
            tParkVoltage.q = tPIParmOutIQ.out;
        }
        /* Limit, if motor is stalled, stop motor commutation */
        if (smc1.omegaFltred < 0)
        {
            uGF.bits.stop = 1;
            /*if motor stall it will reset*/
          
        }
    }
}

inline void SMC_Position_Estimation_Inline(SMC *s)
{

    uint16_t saved_corcon = CORCON;
    CORCON = (CORCON | _CORCON_SATA_MASK | _CORCON_SATB_MASK | _CORCON_ACCSAT_MASK);

    CalcEstI();

    CalcIError();

    /* Sliding control calculator */

    if (_Q15abs(s->iAlphaError) < s->maxSMCError)
    {
        /* s->Zalpha = (s->Kslide * s->IalphaError) / s->MaxSMCError
        /* If we are in the linear range of the slide mode controller,
        /* then correction factor Zalpha will be proportional to the
        /* error (Ialpha - EstIalpha) and slide mode gain, Kslide. */
        CalcZalpha();
    }
    else if (s->iAlphaError > 0)
    {
        s->zAlpha = s->kSlide;
    }
    else
    {
        s->zAlpha = -s->kSlide;
    }
    if (_Q15abs(s->iBetaError) < s->maxSMCError)
    {
        /* s->Zbeta = (s->Kslide * s->IbetaError) / s->MaxSMCError
        /* If we are in the linear range of the slide mode controller,
        /* then correction factor Zbeta will be proportional to the
        /* error (Ibeta - EstIbeta) and slide mode gain, Kslide. */
        CalcZbeta();
    }
    else if (s->iBetaError > 0)
    {
        s->zBeta = s->kSlide;
    }
    else
    {
        s->zBeta = -s->kSlide;
    }
    /* Sliding control filter -> back EMF calculator
    /* s->Ealpha = s->Ealpha + s->Kslf * s->Zalpha -
    /*						   s->Kslf * s->Ealpha
    /* s->Ebeta = s->Ebeta + s->Kslf * s->Zbeta -
    /*						 s->Kslf * s->Ebeta
    /* s->EalphaFinal = s->EalphaFinal + s->KslfFinal * s->Ealpha
    /*								   - s->KslfFinal * s->EalphaFinal
    /* s->EbetaFinal = s->EbetaFinal + s->KslfFinal * s->Ebeta
    /*								 - s->KslfFinal * s->EbetaFinal */
    CalcBEMF();

    /* Rotor angle calculator -> Theta = atan(-EalphaFinal,EbetaFinal) */

    s->theta = atan2CORDIC(-s->eAlphaFinal, s->eBetaFinal);

    accumTheta += s->theta - prevTheta;
    prevTheta = s->theta;

    accumThetaCnt++;
    if (accumThetaCnt == IRP_PERCALC)
    {
        s->omega = accumTheta;
        accumThetaCnt = 0;
        accumTheta = 0;
    }

    /* increment global for open-closed loop transition */
    transCounter++;
    if (transCounter == TRANSITION_STEPS)
    {
        transCounter = 0;
    }
    /*                    Q15(Omega) * 60
    /* Speed RPMs = -----------------------------
    /*               SpeedLoopTime * Motor Poles
    /* For example:
    /*    Omega = 0.5
    /*    SpeedLoopTime = 0.001
    /*    Motor Poles (pole pairs * 2) = 10
    /* Then:
    /*    Speed in RPMs is 3,000 RPMs

    /* s->OmegaFltred = s->OmegaFltred + FilterCoeff * s->Omega
    /*								   - FilterCoeff * s->OmegaFltred */

    CalcOmegaFltred();

    /* Adaptive filter coefficients calculation
    /* Cutoff frequency is defined as 2*_PI*electrical RPS
    /*
    /* 		Wc = 2*_PI*Fc.
    /* 		Kslf = Tpwm*2*_PI*Fc
    /*
    /* Fc is the cutoff frequency of our filter. We want the cutoff frequency
    /* be the frequency of the drive currents and voltages of the motor, which
    /* is the electrical revolutions per second, or eRPS.
    /*
    /* 		Fc = eRPS = RPM * Pole Pairs / 60
    /*
    /* Kslf is then calculated based on user parameters as follows:
    /* First of all, we have the following equation for RPMS:
    /*
    /* 		RPM = (Q15(Omega) * 60) / (SpeedLoopTime * Motor Poles)
    /*		Let us use: Motor Poles = Pole Pairs * 2
    /*		eRPS = RPM * Pole Pairs / 60), or
    /*		eRPS = (Q15(Omega) * 60 * Pole Pairs) / (SpeedLoopTime * Pole Pairs * 2 * 60)
    /*	Simplifying eRPS
    /*		eRPS = Q15(Omega) / (SpeedLoopTime * 2)
    /*	Using this equation to calculate Kslf
    /*		Kslf = Tpwm*2*_PI*Q15(Omega) / (SpeedLoopTime * 2)
    /*	Using diIrpPerCalc = SpeedLoopTime / Tpwm
    /*		Kslf = Tpwm*2*Q15(Omega)*_PI / (diIrpPerCalc * Tpwm * 2)
    /*	Simplifying:
    /*		Kslf = Q15(Omega)*_PI/diIrpPerCalc
    /*
    /* We use a second filter to get a cleaner signal, with the same coefficient
    /*
    /* 		Kslf = KslfFinal = Q15(Omega)*_PI/diIrpPerCalc
    /*
    /* What this allows us at the end is a fixed phase delay for theta compensation
    /* in all speed range, since cutoff frequency is changing as the motor speeds up.
    /*
    /* Phase delay: Since cutoff frequency is the same as the input frequency, we can
    /* define phase delay as being constant of -45 DEG per filter. This is because
    /* the equation to calculate phase shift of this low pass filter is
    /* arctan(Fin/Fc), and Fin/Fc = 1 since they are equal, hence arctan(1) = 45 DEG.
    /* A total of -90 DEG after the two filters implemented (Kslf and KslfFinal). */


    s->kslf = s->kslfFinal = FracMpy(s->omegaFltred, Q15(_PI / IRP_PERCALC));

    /* Since filter coefficients are dynamic, we need to make sure we have a minimum
    /* so we define the lowest operation speed as the lowest filter coefficient */

    if (s->kslf < Q15(OMEGA0 * _PI / IRP_PERCALC))
    {
        s->kslf = Q15(OMEGA0 * _PI / IRP_PERCALC);
        s->kslfFinal = Q15(OMEGA0 * _PI / IRP_PERCALC);
    }
    s->thetaOffset = CONSTANT_PHASE_SHIFT;
    s->theta = s->theta + s->thetaOffset;


    CORCON = saved_corcon;
    return;
}

inline void CalculateParkAngle(void)
{
    smc1.iAlpha = tClarkCurrent.alpha;
    smc1.iBeta = tClarkCurrent.beta;
    smc1.vAlpha = tClarkVoltage.alpha;
    smc1.vBeta = tClarkVoltage.beta;


    SMC_Position_Estimation_Inline(&smc1);

    if (uGF.bits.openLoop)
    {
        if (startupLock < motorParm.lockTime)
        {
            startupLock += 1; /* This variable is incremented until
            /* lock time expires, them the open loop
            /* ramp begins */
        }
        else if (startupRamp < motorParm.endSpeed)
        {
            /* Ramp starts, and increases linearly until endSpeed is reached.
            /* After ramp, estimated theta is used to commutate motor. */
            startupRamp += DELTA_STARTUP_RAMP;
        }
        else
        {
            /* This section enables closed loop, right after open loop ramp. */
            uGF.bits.changeMode = 1;
            uGF.bits.openLoop = 0;
            /* Difference between force angle and estimated theta is saved,
            /* so a soft transition is made when entering closed loop. */
            thetaError = qAngle - smc1.theta;
        }
        qAngle += (int) (startupRamp >> 16);
    }
    else
    {
        /* This value is used to transition from open loop to closed looop.
        /* At the end of open loop ramp, there is a difference between
        /* forced angle and estimated angle. This difference is stored in
        /* thetaError, and added to estimated theta (smc1.Theta) so the
        /* effective angle used for commutating the motor is the same at
        /* the end of open loop, and at the begining of closed loop.
        /* This thetaError is then substracted from estimated theta
        /* gradually in increments of 0.05 degrees until the error is less
        /* than 0.05 degrees. */
        qAngle = smc1.theta + thetaError;
        if ((_Q15abs(thetaError) > _0_05DEG) && (transCounter == 0))
        {
            if (thetaError < 0)
            {
                thetaError += _0_05DEG;
            }
            else
            {
                thetaError -= _0_05DEG;
            }
        }
    }
    return;
}



/*---------------------------------------------------------------------
/* 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, no_auto_psv)) _AD1Interrupt(void)
{

    IFS0bits.AD1IF = 0;

    /* Increment count variable that controls execution
    /* of display and button functions. */
    iADCisrCnt++;

    if (uGF.bits.runMotor)
    {
     MeasCompCurr(ADCBUF_INV_A_IPHASE1, ADCBUF_INV_A_IPHASE2,&measCurrParm);
        /* Calculate qIa,qIb */
     tPhaseCurrents.a = measCurrParm.qIa;
     tPhaseCurrents.b = measCurrParm.qIb;
        
        /* Clarke transform */
        MC_TransformClarke_Assembly(&tPhaseCurrents, &tClarkCurrent);

       /* Calculate commutation angle using estimator */
        CalculateParkAngle();

        /* Park transform */
        MC_TransformPark_Assembly(&tClarkCurrent, &tSinCosTheta, &tParkCurrent);
        /* Calculate control values */
           
        DoControl();

        /* Calculate Sine and Cosine from qAngle */
        MC_CalculateSineCosine_Assembly_Ram(qAngle, &tSinCosTheta);

        /* Calculate vAlpha, Vbeta from Sine, Cosine, Vd and Vq */
        MC_TransformParkInverse_Assembly(&tParkVoltage, &tSinCosTheta, &tClarkVoltage);

        /* Calculate Va,Vb,Vc from vAlpha and Vbeta */
        MC_TransformClarkeInverseSwappedInput_Assembly(&tClarkVoltage, &tPhaseVoltages);

        /* Calculate scaled PWM duty cycles from Va,Vb,Vc and PWM period */
        MC_CalculateSpaceVectorPhaseShifted_Assembly(&tPhaseVoltages, iPwmPeriod, &tPWMDutyValue);
        /* Limit duty cycle per DSXXXXX Errata #XX and write to the PDC registers */
        if (tPWMDutyValue.dutycycle1 < MIN_DUTY)
        {
            INVERTERA_PWM_PDC1 = MIN_DUTY;
        }
        else
        {
            INVERTERA_PWM_PDC1 = tPWMDutyValue.dutycycle1;
        }
        if (tPWMDutyValue.dutycycle2 < MIN_DUTY)
        {
            INVERTERA_PWM_PDC2 = MIN_DUTY;
        }
        else
        {
            INVERTERA_PWM_PDC2 = tPWMDutyValue.dutycycle2;
        }
        if (tPWMDutyValue.dutycycle3 < MIN_DUTY)
        {
            INVERTERA_PWM_PDC3 = MIN_DUTY;
        }
        else
        {
            INVERTERA_PWM_PDC3 = tPWMDutyValue.dutycycle3;
        }
           
    }

#ifdef RTDM
    /********************* DMCI Dynamic Data Views  ***************************/
    /********************** RECORDING MOTOR PHASE VALUES ***************/
    if (DMCIFlags.Recorder)
    {
        SnapShotDelayCnt++;
        if (SnapShotDelayCnt == SnapShotDelay)
        {
            SnapShotDelayCnt = 0;
            *PtrRecBuffer1++ = SNAP1;
            *PtrRecBuffer2++ = SNAP2;
            *PtrRecBuffer3++ = SNAP3;
            *PtrRecBuffer4++ = SNAP4;

            if (PtrRecBuffer4 > RecBuffUpperLimit)
            {
                PtrRecBuffer1 = RecorderBuffer1;
                PtrRecBuffer2 = RecorderBuffer2;
                PtrRecBuffer3 = RecorderBuffer3;
                PtrRecBuffer4 = RecorderBuffer4;
                DMCIFlags.Recorder = 0;
            }
        }
    }
#endif	
    return;
}

/*--------------------------------------------------------------------- */

void SetupControlParameters(void)
{
    /* Turn saturation on to insure that overflows will be handled smoothly. */
    CORCONbits.SATA = 0;
    /* ============= PI D Term =============== */
    tPIParmInID.piState.kp = DKP;
    tPIParmInID.piState.ki = DKI;
    tPIParmInID.piState.kc = DKC;
    tPIParmInID.piState.outMax = DOUTMAX;
    tPIParmInID.piState.outMin = -tPIParmInID.piState.outMax;
    tPIParmInID.piState.integrator = 0;
    tPIParmOutID.out = 0;

    /* ============= PI Q Term =============== */
    tPIParmInIQ.piState.kp = QKP;
    tPIParmInIQ.piState.ki = QKI;
    tPIParmInIQ.piState.kc = QKC;
    tPIParmInIQ.piState.outMax = QOUTMAX;
    tPIParmInIQ.piState.outMin = -tPIParmInIQ.piState.outMax;
    tPIParmInIQ.piState.integrator = 0;
    tPIParmOutIQ.out = 0;

    /* ============= PI W Term =============== */
    tPIParmInWA.piState.kp = WKP;
    tPIParmInWA.piState.ki = WKI;
    tPIParmInWA.piState.kc = WKC;
    tPIParmInWA.piState.outMax = WOUTMAX;
    tPIParmInWA.piState.outMin = -tPIParmInWA.piState.outMax;
    tPIParmInWA.piState.integrator = 0;
    tPIParmOutWA.out = 0;

    /* Setup required parameters

    /* ============= Open Loop ======================
    /* Motor End Speed Calculation
    /* motorParm.endSpeed = ENDSPEEDopenLoop * POLEPAIRS * LOOPTIMEINSEC * 65536 * 65536 / 60.0;
    /* Then, * 65536 which is a right shift done in "void CalculateParkAngle(void)"
    /* ParkParm.qAngle += (int)(startupRamp >> 16); */
    motorParm.endSpeed = END_SPEED_OPEN_LOOP * POLE_PAIRS * LOOPTIME_IN_SEC * 65536 * 65536 / 60.0;
    motorParm.lockTime = LOCKTIME;

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

    /* Scaling constants: Determined by calibration or hardware design. */
    tReadADCParm_A.qK = DQK;

    measCurrParm.qKa = DQKA;
    measCurrParm.qKb = DQKB;
    /* ============= SVGen ===============
    /* Set PWM period to Loop Time */

    iPwmPeriod = LOOPTIME_TCY;
    return;
}


/* NOTE:
/*
/* If Input power supply has switching frequency noise, for example if a
/* switch mode power supply is used, Voltage Ripple Compensation is not
/* recommended, since it will generate spikes on Vd and Vq, which can
/* potentially make the controllers unstable. */

int16_t VoltRippleComp(int16_t Vdq)
{
    int16_t CompVdq;
    /* dcBus is already updated with new DC Bus measurement
    /* in ReadSignedADC0 subroutine.
    /*
    /* If target DC Bus is greater than what we measured last sample, adjust
    /* output as follows:
    /*
    /*                  targetDcBus - dcBus
    /* CompVdq = Vdq + --------------------- * Vdq
    /*                         dcBus
    /*
    /* If Measured dcBus is greater than target, then the following compensation
    /* is implemented:
    /*
    /*            targetDcBus
    /* CompVdq = ------------- * Vdq
    /*               dcBus
    /*
    /* If target and measured are equal, no operation is made. */
    

    if (targetDcBus > dcBus)
    {
        CompVdq = Vdq + FracMpy(FracDiv(targetDcBus - dcBus, dcBus), Vdq);
    }
    else if (dcBus > targetDcBus)
    {
        CompVdq = FracMpy(FracDiv(targetDcBus, dcBus), Vdq);
    }
    else
    {
        CompVdq = Vdq;
    }
    return CompVdq;
}

void InitPeripherals(void)
{
    InitAmplifiersComparator(); /* Initialise comparator and amplifiers */
    InitPWMGenerators(); /*Initialise PWM generators*/
    InitADCModule1(&adcOffset); /* Initialise the ADC used for sensing Inverter A paramneters*/
    InitMeasCompCurr(adcOffset.ch2, adcOffset.ch1, &measCurrParm);

#ifdef ENVOLTRIPPLE

    /* CH0 is AN10 for VDC */
    AD1CHS0bits.CH0SA = 10;
    /* Wait until first conversion takes place to measure offsets. */
    LongDelay(1);
    /* Target DC Bus, without sign. */
    targetDcBus = ((int16_t) ADC1BUF0 >> 1) + Q15(0.5);

    /* CH0 is AN13 for POT */
    AD1CHS0bits.CH0SA = 13;
    uGF.bits.adch0PotOrVdc = 0;

#endif
    LongDelay(100);
}

