// This is code for Local Energy Storage Isolated Full Bridge Converter by Lithium Balance A/S

#include "DSP28x_Project.h"     // Device Headerfile and Examples Include File

// Prototype statements for functions found within this file.
void InitEPwmModules(void);
void InitGpioModules(void);
void UpdateDuty(void);
void InitAdcModules();
__interrupt void adc_isr(void);
void InitOurECan(void);
interrupt void ecan1intB_isr(void);
void ControlLoop(void);
void DisableGateDrivers(void);
void SafetyCheck(void);
void SafeDisable(void);
void TurnLedsOn(void);
void SetupECan(void);
void CheckCan(void);
//void mailbox_read(int16 MBXnbr);
void mailbox_read(void);
void RequestRefChange(float NewRef);

Uint16 TurnOnMargin, TurnOffMargin, TurnOnDelay;    //Safety margins for syncr. rectification.
Uint16 Wa1, Wb1, Wa2, Wb2; //Duty in clock cycles
Uint16 ConversionCount, ControlCounter, ErrorFlag;
Uint16 ControlCounter2;//Counter to make CAN communicate at a lower rate.
Uint16 FBBMode;     //Mode variable to indicate currently used mode
Uint16 Transition; //Transition flag between modes

int message[8];

float32 Kp;
float32 Vs[4];
// Include current sense


float32 VsAvg, VRequestAvg;
float32 ISense[4]; // From version 1 - Current sensing

// Buck or Boost Mode
int mode; // 1 = BOOST 0 = BUCK

Uint16 Wa1Dummy;
float32 ISenseAvg;
float32 VRef;
float32 IRef;
float32 FirstDelay, SecondDelay, ThirdDelay, FirstSum, SecondSum; //Compensator values
float32 DutyFBB;
float32 AntiWindupDelay;
float32 udgang, indgang, indgang_delay, udgang_delay;

// For current control (06-05-2019)
float32 y,y1,y2,x,x1,x2;

float32 Itest;

struct ECAN_REGS ECanbShadow;

//The following are given in clock cycles
#define EPWM1_PERIOD 750//500//1000//750
#define DUTY_MAX 375    // 80% duty -> 600   50% duty -> 375 (Changed 03/05-2019)
#define DUTY_FBB_MIN 0
#define DUTY_BOOST_MIN -3000

#define DUTY_BUCK_MIN  75 // 10% duty
#define DUTY_BUCK_MAX  675 // 90% duty

// Converter Specifics
#define TURNS_RATIO 5

// Safety Limits
#define VS_MAX 650
#define V_REQUEST_MIN -100
#define V_REQUEST_MAX 600
#define IREF_MAX  3

// Slow PI (working)
//#define A_1  1
//#define B_0  2.379e-1
//#define B_1  2.364e-1

// Slow PI-controller by Christian and Anders (03/05-2019)
//#define A_1  1
//#define B_0  0.003504
//#define B_1  0.003447
/*
 *
 * 0.008096 z - 0.008025
  ---------------------
          z - 1
 *
 */

/* Current controller for boost
 *0.987  - 1.937 z^-1 + 0.9502z^-2
  ----------------------------
     1 - 1.519 z^-1 + 0.5186z^-2
 *

  0.02748  - 0.02728z^-1
  -------------------
         1 - z^-1
 */
#define A1  1.519
#define A2  0.5186
#define B0  0.987
#define B1  1.937
#define B2  0.9502


//Anti-windup.
#define AWCoeff 4

int32 debugvar = 0;

void main(void)
{
    //Initialize System Control
    InitSysCtrl();

    //Initialize GPIO:
    InitGpio();
    //Clear all interrupts and initialize PIE vector table:
    // Disable CPU interrupts
    DINT;
    // Initialize PIE control registers to their default state.
    InitPieCtrl();

    // Disable CPU interrupts and clear all CPU interrupt flags:
    IER = 0x0000;
    IFR = 0x0000;

    InitPieVectTable();

    //GPIO - Fan and Disable pin
    InitGpioModules();

    //ePWM
    TurnOffMargin = 0;//Phase offset between ePwm1 and 2 (clock cycles)
    TurnOnMargin = 30; // Possible to reduce this to 15

    TurnOnDelay = TurnOffMargin + TurnOnMargin;
    InitEPwmModules();

    //ADC
    InitAdcModules();

    //CAN
    SetupECan();

    //Initial values stored in delays
    VRef = 0;
    FirstSum = 0.0;
    SecondSum = 0.0;
    FirstDelay = 0;
    SecondDelay = 0;
    ThirdDelay = 0;
    AntiWindupDelay = 0;
    ControlCounter = 0;
    ErrorFlag = 0;
    Kp = 1;
    Itest = 0;
    Wa1Dummy = 0;



    // Selecting mode
    mode = 1; // BOOST = 1 & BUCK = 0
    FBBMode = 1;

    //Initial duty cycle
    DutyFBB = 0.0;
    UpdateDuty();

    udgang = 0.0;
    indgang_delay = 0.0;
    udgang_delay = 0.0;
    indgang = 0.0;


    x = 0.0;
    x1 = 0.0;
    x2 = 0.0;
    y = 0.0;
    y1 = 0.0;
    y2 = 0.0;
    VRef = 1.3; // For current reference

    for (;;)
    {
        asm(" NOP");

    }

}

void InitAdcModules()
{
    //Set ADC clock freq
    EALLOW;
    #if (CPU_FRQ_150MHZ)     // Default - 150 MHz SYSCLKOUT
    #define ADC_MODCLK 0x3 // HSPCLK = SYSCLKOUT/2*ADC_MODCLK2 = 150/(2*3)   = 25.0 MHz
    #endif
    #if (CPU_FRQ_100MHZ)
    #define ADC_MODCLK 0x2 // HSPCLK = SYSCLKOUT/2*ADC_MODCLK2 = 100/(2*2)   = 25.0 MHz
    #endif
    SysCtrlRegs.HISPCP.all = ADC_MODCLK;
    EDIS;

    //Mapping interrupts to our function
    EALLOW;
    PieVectTable.ADCINT = &adc_isr;
    EDIS;
    InitAdc();

    // Enable ADCINT in PIE
    PieCtrlRegs.PIEIER1.bit.INTx6 = 1;
    IER |= M_INT1; // Enable CPU Interrupt 1
    EINT;
    // Enable Global interrupt INTM
    ERTM;
    // Enable Global realtime interrupt DBGM

    // Configure ADC
    AdcRegs.ADCMAXCONV.all = 0x0005;     // Number of conversions = max conv + 1
    AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 8; // Setup ADCINB0 as 1st SEQ1 conv.
    AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 9; // Setup ADCINB1 as 2nd SEQ1 conv.
    AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 10;// B2  -> Vp-
    AdcRegs.ADCCHSELSEQ1.bit.CONV03 = 11;// B3  -> Vs+
    AdcRegs.ADCCHSELSEQ2.bit.CONV04 = 12;// B4  -> Vs-
    AdcRegs.ADCCHSELSEQ2.bit.CONV05 = 13;// B5  -> Iref (not used)
    AdcRegs.ADCTRL2.bit.EPWM_SOCA_SEQ1 = 1; // Enable SOCA from ePWM to start SEQ1
    AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1 = 1;  // Enable SEQ1 interrupt (every EOS)
}

void InitEPwmModules()
{


    // ePWM 1: Primary Side FETs   (Low-Side)
    EPwm1Regs.TBPRD = EPWM1_PERIOD; // Period
    EPwm1Regs.TBPHS.half.TBPHS = 0; // Set Phase register to zero
    EPwm1Regs.TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN; // Symmetrical mode
    EPwm1Regs.TBCTL.bit.PHSEN = TB_DISABLE; // Master module
    EPwm1Regs.TBCTL.bit.PRDLD = TB_SHADOW;
    EPwm1Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1; // TBCLK = SYSCLK
    EPwm1Regs.TBCTL.bit.CLKDIV = TB_DIV1;
    EPwm1Regs.TBCTL.bit.SYNCOSEL = TB_CTR_ZERO; // Sync down-stream module
    EPwm1Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
    EPwm1Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
    EPwm1Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR=Zero
    EPwm1Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO; // load on CTR=Zero

    // Real swicthing

    EPwm1Regs.AQCTLA.bit.ZRO = AQ_SET; // set actions for EPWM1A
    EPwm1Regs.AQCTLA.bit.CBD = AQ_CLEAR;
    EPwm1Regs.AQCTLB.bit.PRD = AQ_SET; // set actions for EPWM1B
    EPwm1Regs.AQCTLB.bit.CAU = AQ_CLEAR;
/*

    //Forcing primary low
    EPwm1Regs.AQCTLA.bit.ZRO = AQ_CLEAR; // set actions for EPWM1A
    EPwm1Regs.AQCTLA.bit.CBD = AQ_CLEAR;
    EPwm1Regs.AQCTLB.bit.PRD = AQ_CLEAR; // set actions for EPWM1B
    EPwm1Regs.AQCTLB.bit.CAU = AQ_CLEAR;
*/


    // ePWM 2: Secondary Side FETs   (High-Side)
    EPwm2Regs.TBPRD = EPWM1_PERIOD; // Period
    EPwm2Regs.TBPHS.half.TBPHS = EPWM1_PERIOD - TurnOffMargin; // Sync to module1 with a phase delay
    EPwm2Regs.TBCTR = 0x0000;   //Clear counter
    EPwm2Regs.TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN; // Symmetrical mode
    EPwm2Regs.TBCTL.bit.PHSEN = TB_ENABLE; //Slave module
    EPwm2Regs.TBCTL.bit.PHSDIR = 0; //Count down after sync
    EPwm2Regs.TBCTL.bit.PRDLD = TB_SHADOW;
    EPwm2Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1; // TBCLK = SYSCLK
    EPwm2Regs.TBCTL.bit.CLKDIV = TB_DIV1;
    EPwm2Regs.TBCTL.bit.SYNCOSEL = TB_CTR_ZERO; // Sync down-stream module

    EPwm2Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
    EPwm2Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
    EPwm2Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR=Zero
    EPwm2Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO; // load on CTR=Zero

    // Adding the switching scheme by Christian 07-05-2019
    EPwm2Regs.AQCTLB.bit.CBD = AQ_SET; // set actions for EPWM2A    // Vi har byttet om på A og B !!!!!!!!!!!!!!!!
    EPwm2Regs.AQCTLB.bit.ZRO = AQ_CLEAR;
    EPwm2Regs.AQCTLA.bit.CAU = AQ_SET; // set actions for EPWM2B
    EPwm2Regs.AQCTLA.bit.PRD = AQ_CLEAR;
/*
    //Forcing secondary side low:
    EPwm2Regs.AQCTLA.bit.CBU = AQ_CLEAR; // set actions for EPWM1A
    EPwm2Regs.AQCTLA.bit.PRD = AQ_CLEAR;
    EPwm2Regs.AQCTLB.bit.CAD = AQ_CLEAR; // set actions for EPWM1B
    EPwm2Regs.AQCTLB.bit.ZRO = AQ_CLEAR;
*/

    // ePWM 3: MORTOR FET ( for boost mode only. not for FBB)
    EPwm3Regs.TBPRD = EPWM1_PERIOD; // Period
    EPwm3Regs.TBPHS.half.TBPHS = 0; // Sync to module2 with a phase delay , zero delay between module 2 and 3
    EPwm3Regs.TBCTR = 0x0000;   //Clear counter
    EPwm3Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP; // Sawtooth mode
    EPwm3Regs.TBCTL.bit.PHSEN = TB_ENABLE; //Slave module
    EPwm3Regs.TBCTL.bit.PHSDIR = 0; //Count down after sync
    EPwm3Regs.TBCTL.bit.PRDLD = TB_SHADOW;
    EPwm3Regs.TBCTL.bit.SYNCOSEL = TB_CTR_ZERO; // Sync down-stream module
    EPwm3Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1; // TBCLK = SYSCLK
    EPwm3Regs.TBCTL.bit.CLKDIV = TB_DIV1;
    EPwm3Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
    EPwm3Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
    EPwm3Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR=Zero
    EPwm3Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO; // load on CTR=Zero
    EPwm3Regs.AQCTLA.bit.CBU = AQ_SET; // set actions for EPWM1A
    EPwm3Regs.AQCTLA.bit.PRD = AQ_CLEAR;
    EPwm3Regs.AQCTLB.bit.CAD = AQ_SET; // set actions for EPWM1B
    EPwm3Regs.AQCTLB.bit.ZRO = AQ_CLEAR;

    //Forcing MORTOR FET low:
    EPwm3Regs.AQCTLA.bit.CBU = AQ_CLEAR; // set actions for EPWM1A
    EPwm3Regs.AQCTLA.bit.PRD = AQ_CLEAR;
    EPwm3Regs.AQCTLB.bit.CAD = AQ_CLEAR; // set actions for EPWM1B
    EPwm3Regs.AQCTLB.bit.ZRO = AQ_CLEAR;

  // Version 2 ADC trigger
    //This PWM module triggers ADC_ISR
    //EPwm4Regs.TBPRD = EPWM1_PERIOD / 8; // Period, a factor of 8 gives 4 times higher freq
    EPwm4Regs.TBPRD = EPWM1_PERIOD / 16; // Period, a factor of 16 gives 8 times higher freq
    EPwm4Regs.TBPHS.half.TBPHS = 0; // Set Phase register to zero
    EPwm4Regs.TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN; // Symmetrical mode
    EPwm4Regs.TBCTL.bit.PHSEN = TB_DISABLE; //Master module
    EPwm4Regs.TBCTL.bit.PRDLD = TB_SHADOW;
    EPwm4Regs.TBCTL.bit.SYNCOSEL = TB_CTR_ZERO; // Sync down-stream module
    EPwm4Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1; // TBCLK = SYSCLK
    EPwm4Regs.TBCTL.bit.CLKDIV = TB_DIV1;
    EPwm4Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
    EPwm4Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
    EPwm4Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR=Zero
    EPwm4Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO; // load on CTR=Zero

    //Triggering ADC
    EPwm4Regs.ETSEL.bit.SOCAEN = 1;        // Enable SOC on A group
    EPwm4Regs.ETSEL.bit.SOCASEL = 1;     // Select SOC from counter = 0
    EPwm4Regs.ETPS.bit.SOCAPRD = 1;        // Generate pulse on every event

/*
    //This PWM module triggers ADC_ISR
        // samples at every other inductor period
        //EPwm4Regs.TBPRD = EPWM1_PERIOD / 8; // Period, a factor of 8 gives 4 times higher freq
        EPwm4Regs.TBPRD = EPWM1_PERIOD; // Same period as switching,
        EPwm4Regs.TBPHS.half.TBPHS = 0; // Phase is same as ePWM2
        EPwm4Regs.TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN; // Symmetrical mode
        EPwm4Regs.TBCTL.bit.PHSEN = TB_ENABLE; //Master module
        EPwm4Regs.TBCTL.bit.PRDLD = TB_SHADOW;
        EPwm4Regs.TBCTL.bit.SYNCOSEL = TB_CTR_ZERO; // Sync down-stream module
        EPwm4Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1; // TBCLK = SYSCLK
        EPwm4Regs.TBCTL.bit.CLKDIV = TB_DIV1;
        EPwm4Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
        EPwm4Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
        EPwm4Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR=Zero
        EPwm4Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO; // load on CTR=Zero

        //Triggering ADC
        EPwm4Regs.ETSEL.bit.SOCAEN = 1;        // Enable SOC on A group
        EPwm4Regs.ETSEL.bit.SOCASEL = 4;     // Starts ADC when pwm counter crosses CMPA (rising)
        EPwm4Regs.ETPS.bit.SOCAPRD = 1;        // Generate pulse on every event
*/

    //Enable pullup
    InitEPwm1Gpio();
    InitEPwm2Gpio();
    InitEPwm3Gpio();
    InitEPwm4Gpio();
}

void InitGpioModules()
{
    EALLOW;

    //Disable pin on gate drivers
    GpioCtrlRegs.GPAPUD.bit.GPIO16 = 0;   // Enable pullup
    GpioDataRegs.GPASET.bit.GPIO16 = 0;   // Set disable low
    GpioCtrlRegs.GPAMUX2.bit.GPIO16 = 0;
    GpioCtrlRegs.GPADIR.bit.GPIO16 = 1;   // Output

    //Fan
    GpioCtrlRegs.GPAPUD.bit.GPIO8 = 0;   // Enable pullup
    GpioDataRegs.GPASET.bit.GPIO8 = 1;   // Turn on fan
    GpioCtrlRegs.GPAMUX1.bit.GPIO8 = 0;
    GpioCtrlRegs.GPADIR.bit.GPIO8 = 1;   // Output

    //LED2, Green LED on MORTOR PCB
    GpioCtrlRegs.GPBPUD.bit.GPIO63 = 0;   // Enable pullup
    GpioDataRegs.GPBSET.bit.GPIO63 = 1;   // 1: Turn on LED
    GpioCtrlRegs.GPBMUX2.bit.GPIO63 = 0;
    GpioCtrlRegs.GPBDIR.bit.GPIO63 = 1;   // Output

    /*//LED 2
    GpioCtrlRegs.GPAPUD.bit.GPIO31 = 0;   // Enable pullup
    GpioDataRegs.GPASET.bit.GPIO31 = 1;   // 1: Turn off LED
    GpioCtrlRegs.GPAMUX2.bit.GPIO31 = 0;
    GpioCtrlRegs.GPADIR.bit.GPIO31 = 1;   // Output

    //LED 3
    GpioCtrlRegs.GPBPUD.bit.GPIO34 = 0;   // Enable pullup
    GpioDataRegs.GPBSET.bit.GPIO34 = 1;   // 1: Turn off LED
    GpioCtrlRegs.GPBMUX1.bit.GPIO34 = 0;
    GpioCtrlRegs.GPBDIR.bit.GPIO34 = 1;   // Output
    */
    EDIS;
}

//Triggered by PWM, fastest
__interrupt void adc_isr(void)
{
    Vs[ConversionCount] = ((float32) (AdcRegs.ADCRESULT3 >> 4) - (float32) (AdcRegs.ADCRESULT4 >> 4)) * 0.2476 + 3.796;

    // Isense is from version 1. By tuning I get: 0.025535375. 2. try: 0.025531375
    if(mode){
    ISense[ConversionCount] = ((float32) (AdcRegs.ADCRESULT0 >> 4)) * 0.025531375 - 51.92982456;
    }
    else{
        ISense[ConversionCount] = ((float32) (AdcRegs.ADCRESULT0 >> 4)) *0.025531375 - 52.32982456;
    }

    //Vs              = ((float32) (AdcRegs.ADCRESULT3 >> 4) - (float32) (AdcRegs.ADCRESULT4 >> 4)) * 0.2476 + 3.196;
    //ISense          = ((float32) (AdcRegs.ADCRESULT0 >> 4)) *0.02569901316 - 51.92982456;
    // Reinitialize for next ADC sequence
    AdcRegs.ADCTRL2.bit.RST_SEQ1 = 1;         // Reset SEQ1
    AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;       // Clear INT SEQ1 bit
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;   // Acknowledge interrupt to PIE

    if (ConversionCount == 3)
    {
        ConversionCount = 0;
        ControlLoop();
    }
    else
    {
        ConversionCount++;
    }

}

//Triggered by adc_isr, slower loop
void ControlLoop(void)
{
    int i;
    //Averaging the oversampled values

    VsAvg = 0;
    ISenseAvg = 0;
    for (i = 0; i < 4; i++){
        VsAvg += Vs[i];
        ISenseAvg +=ISense[i];
    }
    VsAvg = VsAvg / 4;
    ISenseAvg = - ISenseAvg / 4;
    Itest = ISenseAvg;


    // Direct form II current controller:
    IRef = VRef; // The current is not equal the voltage, but it is only for the convenience of the eCan

    if(IRef > IREF_MAX){
            IRef = IREF_MAX;
        }
    /*x = (IRef - ISenseAvg);
    y = (B0*x - B1*x1 + B2*x2 + A1*y1 - A2*y2); // PID-Controller
    //y = (B0*x - B1*x1  + A1*y1); // PI-controller

    // Changing delay:

    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = y;

    DutyFBB = y;
*/
    DutyFBB = VRef;  // Open loop test (03/05-2019) and  (04/05-2019)
    UpdateDuty();


    //MAX code (working)   VOltage control

     //indgang = Kp*(VRef - VsAvg) + AntiWindupDelay;

     /*
     indgang = Kp*(VRef - VsAvg);
     udgang = (indgang*B_0 -indgang_delay*B_1 + udgang_delay*A_1);

     indgang_delay = indgang;
     udgang_delay = udgang;
     //AntiWindupDelay = (DutyFBB - udgang)/AWCoeff;


     DutyFBB = udgang;
 */

    //Checking for CAN message at a lower rate

    if(ControlCounter2 == 9999){
            ControlCounter2 = 0;
            CheckCan();
        }
    ControlCounter2++;
}

//Set DutyFBB right before calling this method
void UpdateDuty(){

    //DutyFBB = DutyFBB*750;   // Only for controls

    if(mode){
    if(DutyFBB > DUTY_MAX){
        DutyFBB = DUTY_MAX;
    }else if(DutyFBB < DUTY_FBB_MIN){
        DutyFBB = DUTY_FBB_MIN;
    }
    }
    else{
     if(DutyFBB > DUTY_BUCK_MAX){
      //   DutyFBB >  DUTY_BUCK_MAX;
    }else if(DutyFBB < DUTY_BUCK_MIN){
          DutyFBB = DUTY_BUCK_MIN;
    }
    }

    Wa1 = (Uint16)DutyFBB;
    Wb1 = EPWM1_PERIOD - Wa1;
    //Wa2 = EPWM1_PERIOD - TurnOnDelay - Wa1;
    //Wb2 = EPWM1_PERIOD + TurnOnDelay - Wb1;

    EPwm1Regs.CMPA.half.CMPA = Wa1; // A duty changed from Wa1 to DUTY_TEST (02-05-2019)
    EPwm1Regs.CMPB = Wb1; // B duty  changed from Wb1 to DUTY_TEST (02-05-2019)
    EPwm2Regs.CMPA.half.CMPA = Wa1;
    EPwm2Regs.CMPB = Wb1;

}

void CheckCan(void)
{
    if(ECanbRegs.CANRMP.all != 0x00000000){
        ECanbRegs.CANRMP.all = 0x00010000;

        //Uncommented because interrupts handle this
        //mailbox_read();//16);
    }
}


void mailbox_read(void)
{
    /*
   void mailbox_read(int16 MBXnbr)
   volatile struct MBOX *MailboxB;

   MailboxB = &ECanbMboxes.MBOX0 + MBXnbr;
   TestMboxB1 = MailboxB->MDL.all; // = 0x9555AAAn (n is the MBX number)
   TestMboxB2 = MailboxB->MDH.all; // = 0x89ABCDEF (a constant)
   TestMboxB3 = MailboxB->MSGID.all;// = 0x9555AAAn (n is the MBX number)

   if(MBXnbr == 16)  // mailbox #1 receive message
   {
   */
   message[0]= ECanbMboxes.MBOX16.MDL.byte.BYTE0;   // read message
   message[1]= ECanbMboxes.MBOX16.MDL.byte.BYTE1;
   message[2]= ECanbMboxes.MBOX16.MDL.byte.BYTE2;
   message[3]= ECanbMboxes.MBOX16.MDL.byte.BYTE3;
   message[4]= ECanbMboxes.MBOX16.MDH.byte.BYTE4;
   message[5]= ECanbMboxes.MBOX16.MDH.byte.BYTE5;
   message[6]= ECanbMboxes.MBOX16.MDH.byte.BYTE6;
   message[7]= ECanbMboxes.MBOX16.MDH.byte.BYTE7;
   ECanbRegs.CANRMP.bit.RMP1 = 1;

   RequestRefChange((float)(message[0]));

   //}


}

interrupt void ecan1intB_isr(void)
{
    /*
    unsigned int mailbox_nr;
//    struct  ECAN_REGS ECanbShadow;      // local copy of CANA registers
    mailbox_nr = ECanbRegs.CANGIF1.bit.MIV1;
    if(mailbox_nr == 16)  // mailbox #1 receive message
    {
    */
        message[0]= ECanbMboxes.MBOX16.MDL.byte.BYTE0;   // read message
        message[1]= ECanbMboxes.MBOX16.MDL.byte.BYTE1;


        message[2]= ECanbMboxes.MBOX16.MDL.byte.BYTE2;
        /*
        message[3]= ECanbMboxes.MBOX16.MDL.byte.BYTE3;
        message[4]= ECanbMboxes.MBOX16.MDH.byte.BYTE4;
        message[5]= ECanbMboxes.MBOX16.MDH.byte.BYTE5;
        message[6]= ECanbMboxes.MBOX16.MDH.byte.BYTE6;
        message[7]= ECanbMboxes.MBOX16.MDH.byte.BYTE7;
   */
        ECanbRegs.CANRMP.bit.RMP1 = 1;

        RequestRefChange((float)(message[0]<<8) + (float)(message[1]));
        Transition = message[2];
    //}
    PieCtrlRegs.PIEACK.bit.ACK9 = 1;
}

void RequestRefChange(float NewRef)
{
    //Setting the reference current
    if(NewRef > V_REQUEST_MAX){
        VRef = V_REQUEST_MAX;
    }else if(NewRef < V_REQUEST_MIN){
        VRef = V_REQUEST_MIN;
    }else{
        VRef = NewRef;
    }
    GpioDataRegs.GPBTOGGLE.bit.GPIO63 = 1;

}


void DisableGateDrivers()
{
    EALLOW;
    //Setting the disable pin on gate drivers high
    GpioCtrlRegs.GPAPUD.bit.GPIO16 = 0;   // Enable pullup
    GpioDataRegs.GPASET.bit.GPIO16 = 1;   // Set disable high
    GpioCtrlRegs.GPAMUX2.bit.GPIO16 = 0;
    GpioCtrlRegs.GPADIR.bit.GPIO16 = 1;   // GPIO6: output
    EDIS;
}

void SafetyCheck(void){
    if(VsAvg > VS_MAX){
        SafeDisable();
    }



}

void SafeDisable(void)
{
    //Force ePWM duty to 0, (each switch is still on 50% of the time)
    ErrorFlag = 1;

    //Force ePWM2 entirely off
    //EPwm2Regs.AQCTLA.bit.CBU = AQ_CLEAR; // set actions for EPWM1A
    //EPwm2Regs.AQCTLA.bit.PRD = AQ_CLEAR;
    //EPwm2Regs.AQCTLB.bit.CAD = AQ_CLEAR; // set actions for EPWM1B
    //EPwm2Regs.AQCTLB.bit.ZRO = AQ_CLEAR;

    TurnLedsOn();   //Turns both LEDs on as a failure indicator
}


void TurnLedsOn(void){
    EALLOW;
    //LED 2
    GpioCtrlRegs.GPAPUD.bit.GPIO31 = 0;   // Enable pullup
    GpioDataRegs.GPASET.bit.GPIO31 = 0;   // 0: Turn on LED
    GpioCtrlRegs.GPAMUX2.bit.GPIO31 = 0;
    GpioCtrlRegs.GPADIR.bit.GPIO31 = 1;   // GPIO6: output

    //LED 3
    GpioCtrlRegs.GPBPUD.bit.GPIO34 = 0;   // Enable pullup
    GpioDataRegs.GPBSET.bit.GPIO34 = 0;   // 0: Turn on LED
    GpioCtrlRegs.GPBMUX1.bit.GPIO34 = 0;
    GpioCtrlRegs.GPBDIR.bit.GPIO34 = 1;   // GPIO6: output
    EDIS;
}

void InitOurECan(void)
{
    EALLOW;

    //
    // Enable internal pull-up for the selected CAN pins
    // Pull-ups can be enabled or disabled by the user.
    // This will enable the pullups for the specified pins.
    // Comment out other unwanted lines.
    //
    //GpioCtrlRegs.GPAPUD.bit.GPIO8 = 0;    //Enable pull-up for GPIO8 (CANTXB)
    //GpioCtrlRegs.GPAPUD.bit.GPIO12 = 0;   //Enable pull-up for GPIO12(CANTXB)
    //GpioCtrlRegs.GPAPUD.bit.GPIO16 = 0;   //Enable pull-up for GPIO16(CANTXB)
    GpioCtrlRegs.GPAPUD.bit.GPIO20 = 0;   //Enable pull-up for GPIO20(CANTXB)

    //GpioCtrlRegs.GPAPUD.bit.GPIO10 = 0;   // Enable pull-up for GPIO10(CANRXB)
    //GpioCtrlRegs.GPAPUD.bit.GPIO13 = 0;  // Enable pull-up for GPIO13(CANRXB)
    //GpioCtrlRegs.GPAPUD.bit.GPIO17 = 0;  // Enable pull-up for GPIO17(CANRXB)
    GpioCtrlRegs.GPAPUD.bit.GPIO21 = 0;  // Enable pull-up for GPIO21(CANRXB)

    //
    // Set qualification for selected CAN pins to asynch only
    // Inputs are synchronized to SYSCLKOUT by default.
    // This will select asynch (no qualification) for the selected pins.
    // Comment out other unwanted lines.
    //
    //GpioCtrlRegs.GPAQSEL1.bit.GPIO10 = 3; // Asynch qual for GPIO10 (CANRXB)
    //GpioCtrlRegs.GPAQSEL1.bit.GPIO13 = 3; // Asynch qual for GPIO13 (CANRXB)
    //GpioCtrlRegs.GPAQSEL2.bit.GPIO17 = 3; // Asynch qual for GPIO17 (CANRXB)
    GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 3; // Asynch qual for GPIO21 (CANRXB)

    //
    // Configure eCAN-B pins using GPIO regs
    // This specifies which of the possible GPIO pins will be eCAN functional
    // pins.
    //
    //GpioCtrlRegs.GPAMUX1.bit.GPIO8 = 2;   // Configure GPIO8 for CANTXB
    //GpioCtrlRegs.GPAMUX1.bit.GPIO12 = 2;  // Configure GPIO12 for CANTXB
    //GpioCtrlRegs.GPAMUX2.bit.GPIO16 = 2;  // Configure GPIO16 for CANTXB
    GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 3;  // Configure GPIO20 for CANTXB

    //GpioCtrlRegs.GPAMUX1.bit.GPIO10 = 2;  // Configure GPIO10 for CANRXB
    //GpioCtrlRegs.GPAMUX1.bit.GPIO13 = 2;  // Configure GPIO13 for CANRXB
    //GpioCtrlRegs.GPAMUX2.bit.GPIO17 = 2;  // Configure GPIO17 for CANRXB
    GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 3;  // Configure GPIO21 for CANRXB

    EDIS;
}



void SetupECan(void)
{

    // eCAN control registers require read/write access using 32-bits.  Thus we
    // will create a set of shadow registers for this example.  These shadow
    // registers will be used to make sure the access is 32-bits and not 16.


    // Step 1. Initialize System Control:
    // PLL, WatchDog, enable Peripheral Clocks
    // This example function is found in the DSP2833x_SysCtrl.c file.
       //InitSysCtrl();

    // Step 2. Initialize GPIO:
    // This example function is found in the DSP2833x_Gpio.c file and
    // illustrates how to set the GPIO to it's default state.
    //-   InitGpio();  // Skipped for this example

    // For this example, configure CAN pins using GPIO regs here
    // This function is found in DSP2833x_ECan.c
       //InitECanGpio();
       InitOurECan();
    // Step 3. Clear all interrupts and initialize PIE vector table:
    // Disable CPU interrupts
    //-   DINT;

    // Initialize PIE control registers to their default state.
    // The default state is all PIE interrupts disabled and flags
    // are cleared.
    // This function is found in the DSP2833x_PieCtrl.c file.
       //InitPieCtrl();

    // Disable CPU interrupts and clear all CPU interrupt flags:
       IER = 0x0000;
       IFR = 0x0000;

    // Initialize the PIE vector table with pointers to the shell Interrupt
    // Service Routines (ISR).
    // This will populate the entire table, even if the interrupt
    // is not used in this example.  This is useful for debug purposes.
    // The shell ISR routines are found in DSP2833x_DefaultIsr.c.
    // This function is found in DSP2833x_PieVect.c.
       //InitPieVectTable();

    // Step 4. Initialize all the Device Peripherals:
    // This function is found in DSP2833x_InitPeripherals.c
    //   InitPeripherals(); // Not required for this example

    // Step 5. User specific code, enable interrupts:


        InitECanb();


        //Interrupt stuff
        EALLOW;
        PieVectTable.ECAN1INTB = &ecan1intB_isr;    // re - map CAN INT1
        ECanbShadow.CANMIM.all = 0;
        ECanbShadow.CANMIM.bit.MIM16  = 1;  // MB#5  Mailbox interrupt is enabled
        ECanbRegs.CANMIM.all = ECanbShadow.CANMIM.all;
        ECanbShadow.CANMIL.all = 0;
        ECanbShadow.CANMIL.bit.MIL16  = 1;  // Int.-Level MB#5  -> I1EN
        ECanbRegs.CANMIL.all = ECanbShadow.CANMIL.all;
        ECanbShadow.CANGIM.all = 0;
        ECanbShadow.CANGIM.bit.I1EN = 1;  // enable I1EN
        ECanbRegs.CANGIM.all = ECanbShadow.CANGIM.all;

        PieCtrlRegs.PIEIER9.bit.INTx8 = 1;  // ECAN1INTB
        IER = 0x0101;       // enable core line INT1
        EINT;               // enable global interrupt INTM
        ERTM;               // Enable global real time DBGM
        CpuTimer0Regs.TCR.bit.TSS = 0;  // start T0
        EDIS;


        //Disable mailbox before changing
        ECanbRegs.CANME.all = 0x00000000;

        // Mailboxes can be written to 16-bits or 32-bits at a time
        // Write to the MSGID field of TRANSMIT mailboxes MBOX0 - 15
        // Start ID field with 0x8
        ECanbMboxes.MBOX0.MSGID.all = 0x800001A4; //420

        // Write to the MSGID field of RECEIVE mailboxes MBOX16 - 31
        ECanbMboxes.MBOX16.MSGID.all = 0x00240000;//shift ID by two bits when not using extended identifier (fx. ID = 9 -> write 36 (24 in hex))

    //    ECanbMboxes.MBOX18.MSGID.bit.AME = 1; //Acceptance mask enable
    //    ECanbMboxes.MBOX18.MSGID.bit.IDE = 1; //we are using extended identifier
    //    ECanbLAMRegs.LAM18.all = 0xFFFFFFFF; //Setting LAM bits high -> accepting all IDs

        // Configure Mailboxes 0-15 as Tx, 16-31 as Rx
        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        ECanbRegs.CANMD.all = 0xFFFF0000;

        // Enable all Mailboxes */
        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        ECanbRegs.CANME.all = 0xFFFFFFFF;


        // Specify that 8 bits will be sent/received
        ECanbMboxes.MBOX0.MSGCTRL.bit.DLC = 8;
        ECanbMboxes.MBOX16.MSGCTRL.bit.DLC = 8;

        //Data to be transmitted
        // Write to the mailbox RAM field of MBOX0 - 15
        ECanbMboxes.MBOX0.MDL.all = 0;
        ECanbMboxes.MBOX0.MDH.all = 100;

        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        EALLOW;
        ECanbRegs.CANMIM.all = 0xFFFFFFFF;
        EDIS;

}