//-----------------------------------------
//  Includes
//-----------------------------------------

#include <xdc/std.h>                                //mandatory - have to include first, for BIOS types
#include <ti/sysbios/BIOS.h>                        //mandatory - if you call APIs like BIOS_start()
#include <xdc/runtime/Log.h>                        //for Log_info() calls when UIA is added
#include <xdc/cfg/global.h>                         //header file for statically defined objects/handles
#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>
#include <ti/sysbios/knl/Task.h>
#include <Queue.h>
#include "F2806x_Device.h"
#include <F2806x_GlobalPrototypes.h>
#include <F2806x_CpuTimers.h>
#include <F2806x_Examples.h>
#include <math.h>
#include "DSP28x_Project.h"                         // Device Headerfile and Examples Include File

//-----------------------------------------
// Prototypes
//-----------------------------------------
void hardware_init(void);
void InitXINT1(void);
void InitXINT2(void);
void InitCAN(void);
void InitECan(void);
void InitECanGpio(void);
void InitECapture(void);
void InitEPwmTimer(void);

//-----------------------------------------
// Static Globals
//-----------------------------------------

#define CAN_TIMEOUT 50
#define REV 0
#define FWD 1
#define HIGHPERIOD 0
#define PERIOD 1

//-----------------------------------------
// Dynamic Globals
//-----------------------------------------

unsigned long MessageReceivedCount = 0;
unsigned long MessageTransmittedCount = 0;
// long BO_counter = 0 ; //Counts the # of times node enters BO
// unsigned long long CAN_Data[10];

unsigned int Period = 67;           //1Hz
unsigned int SensorHighPeriod = 0;
unsigned int SensorPeriod = 0;
double SensorAngleGlobal = 0.0;
double SensorDutyCycleGlobal = 0.0;
unsigned short MotorLimitGlobal = 0;
unsigned short RollMotorDirection = FWD;
unsigned short TiltMotorDirection = FWD;

struct ECAN_REGS ECanaShadow;

typedef struct SnrObj {
    unsigned int HighPeriod;
    unsigned int Period;
} SnrObj, *Snr;




//-----------------------------------------------------------------------------
// hardware_init()
//-----------------------------------------------------------------------------
void hardware_init(void)                    //called by main
{

// Init PLL, watchdog, periph clocks - see F2806x_SysCtrl.c file
// Clock frequency set to 90 MHz - see F2806x_Examples.h
    InitSysCtrl();

// Copy InitFlash fxn to RAM and run it - sets flash wait states for 90MHz
    memcpy(&RamfuncsRunStart,&RamfuncsLoadStart,(unsigned long)&RamfuncsLoadSize);
    InitFlash();

    // Step 2. Initalize GPIO:
    // This example function is found in the F2806x_Gpio.c file and
    // illustrates how to set the GPIO to it's default state.
    // InitGpio();
    // Setup only the GP I/O only for SCI-A and SCI-B functionality
    // This function is found in F2806x_Sci.c
    InitSciaGpio();         // Configure the the GPIO for SCIa (COMa) for the terminal console
    InitScibGpio();         // Configure the the GPIO for SCIb (COMb) for the Xbee radio

    InitXINT1();            // Configure the external interrupt registers (GPIO1)
    InitXINT2();            // Configure the external interrupt registers (GPIO2)

    InitECapture();         // Configure the external capture of the sensor PWM output
    InitEPwmTimer();
    InitCAN();              // Configure the CAN registers

}

void InitSciaGpio()
{
    // Configure GPIO21 (COM) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 0;
        GpioCtrlRegs.GPADIR.bit.GPIO21 = 1;
        EDIS;

    // Configure GPIO6 (J8-pin80) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 0;
        GpioCtrlRegs.GPADIR.bit.GPIO6 = 1;
        EDIS;

    // Configure GPIO7 (J8-pin79) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPAMUX1.bit.GPIO7 = 0;
        GpioCtrlRegs.GPADIR.bit.GPIO7 = 1;
        EDIS;

    // Configure GPIO8 (J8-pin78) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPAMUX1.bit.GPIO8 = 0;
        GpioCtrlRegs.GPADIR.bit.GPIO8 = 1;
        EDIS;

    // Configure GPIO9 (J8-pin77) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPAMUX1.bit.GPIO9 = 0;
        GpioCtrlRegs.GPADIR.bit.GPIO9 = 1;
        EDIS;

   EALLOW;

/* Enable internal pull-up for the selected pins */
// Pull-ups can be enabled or disabled disabled by the user.
// This will enable the pullups for the specified pins.

   GpioCtrlRegs.GPAPUD.bit.GPIO28 = 0;      // Enable pull-up for GPIO28 (SCIRXDA)
   GpioCtrlRegs.GPAPUD.bit.GPIO29 = 0;      // Enable pull-up for GPIO29 (SCITXDA)
//   GpioCtrlRegs.GPAPUD.bit.GPIO7 = 0;     // Enable pull-up for GPIO7  (SCIRXDA)
//   GpioCtrlRegs.GPAPUD.bit.GPIO12 = 0;    // Enable pull-up for GPIO12 (SCITXDA)

/* Set qualification for selected pins to asynch only */
// Inputs are synchronized to SYSCLKOUT by default.
// This will select asynch (no qualification) for the selected pins.

   GpioCtrlRegs.GPAQSEL2.bit.GPIO28 = 3;    // Asynch input GPIO28 (SCIRXDA)
//   GpioCtrlRegs.GPAQSEL1.bit.GPIO7 = 3;   // Asynch input GPIO7 (SCIRXDA)

/* Configure SCI-A pins using GPIO regs*/
// This specifies which of the possible GPIO pins will be SCI functional pins.

   GpioCtrlRegs.GPAMUX2.bit.GPIO28 = 1;     // Configure GPIO28 for SCIRXDA operation
   GpioCtrlRegs.GPAMUX2.bit.GPIO29 = 1;     // Configure GPIO29 for SCITXDA operation
//   GpioCtrlRegs.GPAMUX1.bit.GPIO7 = 2;    // Configure GPIO7  for SCIRXDA operation
//   GpioCtrlRegs.GPAMUX1.bit.GPIO12 = 2;   // Configure GPIO12 for SCITXDA operation

    EDIS;
}

void InitScibGpio()
{
    // Configure GPIO34 (LED9) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPBMUX1.bit.GPIO34 = 0;    // Enable GPIO for GPIO34
        GpioCtrlRegs.GPBDIR.bit.GPIO34 = 1;     // Set Direction as Output GPIO for GPIO34
        EDIS;

    // Configure GPIO39 (LED10) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPBMUX1.bit.GPIO39 = 0;    // Enable GPIO for GPIO39
        GpioCtrlRegs.GPBDIR.bit.GPIO39 = 1;     // Set Direction as Output GPIO for GPIO39
        EDIS;

    // Configure GPIO20 (PROX, Quick Coupler switch) as a GPIO output pin
        EALLOW;
        GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 0;
        GpioCtrlRegs.GPADIR.bit.GPIO20 = 1;
        EDIS;

       EALLOW;

    /* Enable internal pull-up for the selected pins */
    // Pull-ups can be enabled or disabled disabled by the user.
    // This will enable the pullups for the specified pins.

        GpioCtrlRegs.GPAPUD.bit.GPIO11 = 0;    // Enable pull-up for GPIO11 (SCIRXDB)
    //  GpioCtrlRegs.GPAPUD.bit.GPIO15 = 0;    // Enable pull-up for GPIO15 (SCIRXDB)
    //  GpioCtrlRegs.GPAPUD.bit.GPIO19 = 0;    // Enable pull-up for GPIO19 (SCIRXDB)
    //  GpioCtrlRegs.GPAPUD.bit.GPIO23 = 0;    // Enable pull-up for GPIO23 (SCIRXDB)
    //  GpioCtrlRegs.GPBPUD.bit.GPIO41 = 0;    // Enable pull-up for GPIO41 (SCIRXDB)
    //  GpioCtrlRegs.GPBPUD.bit.GPIO44 = 0;    // Enable pull-up for GPIO44 (SCIRXDB)

        GpioCtrlRegs.GPAPUD.bit.GPIO9 = 0;     // Enable pull-up for GPIO9 (SCITXDB)
    //  GpioCtrlRegs.GPAPUD.bit.GPIO14 = 0;    // Enable pull-up for GPIO14 (SCITXDB)
    //  GpioCtrlRegs.GPAPUD.bit.GPIO18 = 0;    // Enable pull-up for GPIO18 (SCITXDB)
    //  GpioCtrlRegs.GPAPUD.bit.GPIO22 = 0;    // Enable pull-up for GPIO22 (SCITXDB)
    //  GpioCtrlRegs.GPBPUD.bit.GPIO40 = 0;    // Enable pull-up for GPIO40 (SCITXDB)
    //  GpioCtrlRegs.GPBPUD.bit.GPIO58 = 0;    // Enable pull-up for GPIO58 (SCITXDB)

    /* Set qualification for selected pins to asynch only */
    // Inputs are synchronized to SYSCLKOUT by default.
    // This will select asynch (no qualification) for the selected pins.

        GpioCtrlRegs.GPAQSEL1.bit.GPIO11 = 3;  // Asynch input GPIO11 (SCIRXDB)
    //  GpioCtrlRegs.GPAQSEL1.bit.GPIO15 = 3;  // Asynch input GPIO15 (SCIRXDB)
    //  GpioCtrlRegs.GPAQSEL2.bit.GPIO19 = 3;  // Asynch input GPIO19 (SCIRXDB)
    //  GpioCtrlRegs.GPAQSEL2.bit.GPIO23 = 3;  // Asynch input GPIO23 (SCIRXDB)
    //  GpioCtrlRegs.GPBQSEL1.bit.GPIO41 = 3;  // Asynch input GPIO41 (SCIRXDB)
    //  GpioCtrlRegs.GPBQSEL1.bit.GPIO44 = 3;  // Asynch input GPIO44 (SCIRXDB)


    /* Configure SCI-B pins using GPIO regs*/
    // This specifies which of the possible GPIO pins will be SCI functional pins.

        GpioCtrlRegs.GPAMUX1.bit.GPIO11 = 2;   // Configure GPIO11 for SCIRXDB operation
    //  GpioCtrlRegs.GPAMUX1.bit.GPIO15 = 2;   // Configure GPIO15 for SCIRXDB operation
    //  GpioCtrlRegs.GPAMUX2.bit.GPIO19 = 2;   // Configure GPIO19 for SCIRXDB operation
    //  GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 3;   // Configure GPIO23 for SCIRXDB operation
    //  GpioCtrlRegs.GPBMUX1.bit.GPIO41 = 2;   // Configure GPIO41 for SCIRXDB operation
    //  GpioCtrlRegs.GPBMUX1.bit.GPIO44 = 2;   // Configure GPIO44 for SCIRXDB operation


        GpioCtrlRegs.GPAMUX1.bit.GPIO9 = 2;     // Configure GPIO9 for SCITXDB operation
    //  GpioCtrlRegs.GPAMUX1.bit.GPIO14 = 2;    // Configure GPIO14 for SCITXDB operation
    //  GpioCtrlRegs.GPAMUX2.bit.GPIO18 = 2;    // Configure GPIO18 for SCITXDB operation
    //  GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 3;    // Configure GPIO22 for SCITXDB operation
    //  GpioCtrlRegs.GPBMUX1.bit.GPIO40 = 2;    // Configure GPIO40 for SCITXDB operation
    //  GpioCtrlRegs.GPBMUX2.bit.GPIO58 = 2;    // Configure GPIO58 for SCITXDB operation


        EDIS;
}

void InitXINT1(void)
{
    // GPIO0 is an input
       EALLOW;
       GpioCtrlRegs.GPAMUX1.bit.GPIO0 = 0;          // Use GPIO 0
       GpioCtrlRegs.GPADIR.bit.GPIO0 = 0;           // Set GPIO 0 as input
       GpioCtrlRegs.GPAPUD.bit.GPIO0 = 0;           // Enable pull-up for GPIO0
       EDIS;

       EALLOW;
       GpioCtrlRegs.GPAMUX2.bit.GPIO18 = 0;          // Use GPIO 18
       GpioCtrlRegs.GPADIR.bit.GPIO18 = 0;           // Set GPIO 18 as input
       GpioCtrlRegs.GPAPUD.bit.GPIO18 = 0;           // Enable pull-up for GPIO18
       EDIS;

       EALLOW;
       GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 0;          // Use GPIO 22
       GpioCtrlRegs.GPADIR.bit.GPIO22 = 0;           // Set GPIO 22 as input
       GpioCtrlRegs.GPAPUD.bit.GPIO22 = 0;           // Enable pull-up for GPIO18
       EDIS;

       EALLOW;
       GpioCtrlRegs.GPBMUX1.bit.GPIO33 = 0;          // Use GPIO 33
       GpioCtrlRegs.GPBDIR.bit.GPIO33 = 0;           // Set GPIO 33 as input
       GpioCtrlRegs.GPBPUD.bit.GPIO33 = 0;           // Enable pull-up for GPIO18
       EDIS;

    // GPIO0 is XINT1
       EALLOW;
       GpioIntRegs.GPIOXINT1SEL.bit.GPIOSEL = 0;   // Set GPIO 0 as a XINT1 (external interrupt)
       EDIS;

    // Configure XINT1
       XIntruptRegs.XINT1CR.bit.POLARITY = 0;      // Falling edge interrupt

    // Enable XINT1
       XIntruptRegs.XINT1CR.bit.ENABLE = 1;        // Enable XINT1 (external interrupt)
}

void InitXINT2(void)
{
    // GPIO1 is an input
       EALLOW;
       GpioCtrlRegs.GPAMUX1.bit.GPIO1 = 0;          // Use GPIO 1
       GpioCtrlRegs.GPADIR.bit.GPIO1 = 0;           // Set GPIO 1 as input
       GpioCtrlRegs.GPAPUD.bit.GPIO1 = 0;           // Enable pull-up for GPIO1
       EDIS;

    // GPIO1 is XINT2
       EALLOW;
       GpioIntRegs.GPIOXINT2SEL.bit.GPIOSEL = 1;   // Set GPIO 1 as a XINT1 (external interrupt)
       EDIS;

    // Configure XINT2
       XIntruptRegs.XINT2CR.bit.POLARITY = 0;      // Falling edge interrupt

    // Enable XINT2
       XIntruptRegs.XINT2CR.bit.ENABLE = 1;        // Enable XINT1 (external interrupt)
}

void InitECapture()
{
    InitECap1Gpio();

// Step 3. Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
    DINT;

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

    ECap1Regs.ECEINT.all = 0x0000;             // Disable all capture interrupts
    ECap1Regs.ECCLR.all = 0xFFFF;              // Clear all CAP interrupt flags
    ECap1Regs.ECCTL1.bit.CAPLDEN = 0;          // Disable CAP1-CAP4 register loads
    ECap1Regs.ECCTL2.bit.TSCTRSTOP = 0;        // Make sure the counter is stopped

    // Configure peripheral registers
    ECap1Regs.ECCTL2.bit.CONT_ONESHT = 1;      // One-shot
    ECap1Regs.ECCTL2.bit.STOP_WRAP = 3;        // Stop at 4 events
    ECap1Regs.ECCTL1.bit.CAP1POL = 1;          // Falling edge
    ECap1Regs.ECCTL1.bit.CAP2POL = 0;          // Rising edge
    ECap1Regs.ECCTL1.bit.CAP3POL = 1;          // Falling edge
    ECap1Regs.ECCTL1.bit.CAP4POL = 0;          // Rising edge
    ECap1Regs.ECCTL1.bit.CTRRST1 = 1;          // Difference operation
    ECap1Regs.ECCTL1.bit.CTRRST2 = 1;          // Difference operation
    ECap1Regs.ECCTL1.bit.CTRRST3 = 1;          // Difference operation
    ECap1Regs.ECCTL1.bit.CTRRST4 = 1;          // Difference operation
    ECap1Regs.ECCTL2.bit.SYNCI_EN = 1;         // Enable sync in
    ECap1Regs.ECCTL2.bit.SYNCO_SEL = 0;        // Pass through
    ECap1Regs.ECCTL1.bit.CAPLDEN = 1;          // Enable capture units

//    ECap1Regs.ECCTL1.bit.CAP1POL = 0;       //EC_RISING
//    ECap1Regs.ECCTL1.bit.CAP2POL = 1;       //EC_FALLING
//    ECap1Regs.ECCTL1.bit.CAP3POL = 0;       //EC_RISING
//    ECap1Regs.ECCTL1.bit.CAP4POL = 1;       //EC_FALLING
//    ECap1Regs.ECCTL1.bit.CTRRST1 = 0;       //EC_ABS_MODE
//    ECap1Regs.ECCTL1.bit.CTRRST2 = 0;       //EC_ABS_MODE
//    ECap1Regs.ECCTL1.bit.CTRRST3 = 0;       //EC_ABS_MODE
//    ECap1Regs.ECCTL1.bit.CTRRST4 = 0;       //EC_ABS_MODE
//    ECap1Regs.ECCTL1.bit.CAPLDEN = 1;       //EC_ENABLE
//    ECap1Regs.ECCTL1.bit.PRESCALE = 0;      //EC_DIV1
//    ECap1Regs.ECCTL2.bit.CAP_APWM = 0;      //EC_CAP_MODE
//    ECap1Regs.ECCTL2.bit.CONT_ONESHT = 0;   //EC_CONTINUOUS
//    ECap1Regs.ECCTL2.bit.SYNCO_SEL = 2;     //EC_SYNCO_DIS
//    ECap1Regs.ECCTL2.bit.SYNCI_EN = 0;      //EC_DISABLE
//    ECap1Regs.ECCTL2.bit.TSCTRSTOP = 1;     //EC_RUN - Allow TSCTR to run

    ECap1Regs.ECCTL2.bit.TSCTRSTOP = 1;        // Start Counter
    ECap1Regs.ECCTL2.bit.REARM = 1;            // arm one-shot
    ECap1Regs.ECCTL1.bit.CAPLDEN = 1;          // Enable CAP1-CAP4 register loads
    ECap1Regs.ECEINT.bit.CEVT4 = 1;            // 4 events = interrupt

// Enable CPU INT4 which is connected to ECAP1-4 INT:
    IER |= M_INT4;

// Enable eCAP INTn in the PIE: Group 3 interrupt 1-6
    PieCtrlRegs.PIEIER4.bit.INTx1 = 1;

// Enable global Interrupts and higher priority real-time debug events:
    EINT;   // Enable Global interrupt INTM
    ERTM;   // Enable Global realtime interrupt DBGM
}

void InitEPwmTimer()
{
   InitEPwm3Gpio();

   EALLOW;
   SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 0;
   EDIS;

   EPwm3Regs.TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN; // Count up/down on J4-Pin35
   EPwm3Regs.TBPRD = 1000;  // Period of ePWM
   EPwm3Regs.TBPHS.all = 0x00000000;
   EPwm3Regs.AQCTLB.bit.PRD = AQ_TOGGLE;      // Toggle on PRD

   // TBCLK = SYSCLKOUT
   EPwm3Regs.TBCTL.bit.HSPCLKDIV = 7;
   EPwm3Regs.TBCTL.bit.CLKDIV = 7;

   EALLOW;
   SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1;
   EDIS;

}

void InitCAN(void)
{
    /* Create a shadow register structure for the CAN control registers.

    This is needed, since, only 32-bit access is allowed to these registers. 16-bit access
     to these registers could potentially corrupt the register contents. This is
     especially true while writing to a bit (or group of bits) among bits 16 - 31 */

   /* Initialize the CAN module */
    InitECan();
    InitECanGpio();

    MessageReceivedCount = 0;

    /* Write to the MSGID field  */

    EALLOW;
//  ECanaMboxes.MBOX0.MSGID.all = 0x80F00400;
//  ECanaMboxes.MBOX1.MSGID.all = 0xC0F00400;
    ECanaMboxes.MBOX0.MSGID.all = 0x80EF2717;       // Transmit Read Request for Quick Coupler
    ECanaMboxes.MBOX1.MSGID.all = 0xC0EF1727;       // Recieve Read Response for Quick Coupler
    ECanaMboxes.MBOX2.MSGID.all = 0x80FDC527;       // Transmit Work Tool ID (PGN 64965/SPN 2902)

    /* Configure mask for Mailboxes*/
    ECanaLAMRegs.LAM1.all = 0xBF000000;             // MSGID Mask for MBOX1 (RX Quick Coupler): b10111111000000000000000000000000

    /* Configure bit timing parameters for eCANA*/
    ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;    // Save the (CANMC) Master Control Register
    ECanaShadow.CANMC.bit.CCR = 1 ;                 // Set CCR = 1
    ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;    // Set the (CANMC) Master Control Register

    // Wait until the CPU has been granted permission to change the configuration registers
    do
    {
      ECanaShadow.CANES.all = ECanaRegs.CANES.all;  // Set the (CANES) Error and Status Register
    } while(ECanaShadow.CANES.bit.CCE != 1 );       // Wait for CCE bit to be set..

    ECanaShadow.CANBTC.all = 0;
    /* The following block is for 90 MHz SYSCLKOUT. (45 MHz CAN module clock Bit rate = 1 Mbps
       See Note at end of file. */

    ECanaShadow.CANBTC.bit.BRPREG = 11;             // 2 for 1 Mbps, 5 for 500 kbps, 11 for 250 kbps, 23 for 125 kbps, 29 for 100 kbps
    ECanaShadow.CANBTC.bit.TSEG2REG = 3;            // Time Segement 2 for the Bit Timing Configuration Register (length of the phase in TQ units) TSEG2 = TSEG2reg + 1
    ECanaShadow.CANBTC.bit.TSEG1REG = 9;            // Time Segement 1 for the Bit Timing Configuration Register (length of a bit on the CAN bus) TSEG1 = TSEG1reg + 1

    ECanaShadow.CANBTC.bit.SAM = 1;                 // The CAN module samples three times and make a majority decision.
    ECanaRegs.CANBTC.all = ECanaShadow.CANBTC.all;  // Set the (CANBTC) Bit Timing Configuration Register

    ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;    // Save the (CANMC) Master Control Register
    ECanaShadow.CANMC.bit.CCR = 0 ;                 // Clear CCR = 0: The CPU requests normal operation.
    ECanaShadow.CANMC.bit.ABO = 1;                  // Set ABO = 1: Auto bus on.
    ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;    // Set the (CANMC) Master Control Register

    // Wait until the CPU no longer has permission to change the configuration registers
    do
    {
      ECanaShadow.CANES.all = ECanaRegs.CANES.all;  // Set the (CANES) Error and Status Register
    } while(ECanaShadow.CANES.bit.CCE != 0 );       // Wait for CCE bit to be  cleared..

    /* Configure Mailbox 0 under test as a Transmit mailbox */
    ECanaShadow.CANMD.all = ECanaRegs.CANMD.all;    // Save the (CANMD) Message Data Register
    ECanaShadow.CANMD.bit.MD0 = 0;                  // Set Mailbox 0 as transmit
    ECanaRegs.CANMD.all = ECanaShadow.CANMD.all;    // Set the (CANMD) Message Data Register

    /* Enable Mailbox 0 under test */
    ECanaShadow.CANME.all = ECanaRegs.CANME.all;    // Save the (CANME) Mailbox Enable Register
    ECanaShadow.CANME.bit.ME0 = 1;                  // Enable Mailbox 0
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;    // Set the (CANME) Mailbox Enable Register

    /* Configure Mailbox 1 under test as a Recieve mailbox */
    ECanaMboxes.MBOX1.MSGID.bit.IDE = 1;            // Set Mailbox 1 with a 29-bit extended MSGID
    ECanaShadow.CANMD.all = ECanaRegs.CANMD.all;    // Save the (CANMD) Message Data Register
    ECanaShadow.CANMD.bit.MD1 = 1;                  // Set Mailbox 1 as recieve
    ECanaRegs.CANMD.all = ECanaShadow.CANMD.all;    // Set the (CANMD) Message Data Register

    /* Enable Mailbox 1 under test */
    ECanaShadow.CANME.all = ECanaRegs.CANME.all;    // Save the (CANME) Mailbox Enable Register
    ECanaShadow.CANME.bit.ME1 = 1;                  // Enable Mailbox 1
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;    // Set the (CANME) Mailbox Enable Register

    /* Configure Mailbox 2 under test as a Transmit mailbox */
    ECanaShadow.CANMD.all = ECanaRegs.CANMD.all;    // Save the (CANMD) Message Data Register
    ECanaShadow.CANMD.bit.MD2= 0;                   // Set Mailbox 2 as transmit
    ECanaRegs.CANMD.all = ECanaShadow.CANMD.all;    // Set the (CANMD) Message Data Register

    /* Enable Mailbox 2 under test */
    ECanaShadow.CANME.all = ECanaRegs.CANME.all;    // Set the (CANME) Mailbox Enable Register
    ECanaShadow.CANME.bit.ME2 = 1;                  // Enable Mailbox 2
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;    // Save the (CANME) Mailbox Enable Register

    /* Write to DLC field in Master Control reg */
    ECanaMboxes.MBOX0.MSGCTRL.bit.DLC = 8;          // Set the DLC to 8 Bytes for Mailbox 0
    ECanaMboxes.MBOX1.MSGCTRL.bit.DLC = 8;          // Set the DLC to 8 Bytes for Mailbox 1
    ECanaMboxes.MBOX2.MSGCTRL.bit.DLC = 8;          // Set the DLC to 8 Bytes for Mailbox 2

    EALLOW;
        ECanaRegs.CANMIL.bit.MIL1=0;                // Clear the Mailbox Interrupt Level bit
        ECanaRegs.CANGIM.bit.I0EN=1;                // Set the Global Mailbox Interrupt Mask bit
        ECanaRegs.CANMIM.all = 0x00000002;          // Set the Mailbox Interrupt Mask Register for Mailbox 1
    EDIS;
}

//-----------------------------------------
//  ======== TiltMotorLimitHWI ========
//-----------------------------------------
Void TiltMotorLimitHWI()
{
      GpioDataRegs.GPBTOGGLE.bit.GPIO39 = 1;             // toggle LED10 using "TOGGLE" (1 = ON)

      if(TiltMotorDirection == FWD)
          Semaphore_post(TiltMotorFwdLimit); // The Forward limit of the Roll motor has been reached
      else if(TiltMotorDirection == REV)
          Semaphore_post(TiltMotorRevLimit); // The Reverse limit of the Roll motor has been reached
      else
      {
          System_printf("TiltMotorDirection not determined\n");
          System_flush(); // force SysMin output to console
      }
}

//-----------------------------------------
//  ======== TiltMotorDriverTask ========
//-----------------------------------------
Void TiltMotorDriverTask()
{
    unsigned short MotorFwdLimit = 0;
    unsigned short MotorRevLimit = 0;

    while(1)
    {

        System_printf("enter TiltMotorDriverTask()\n");

        MotorFwdLimit = Semaphore_pend(TiltMotorFwdLimit, BIOS_NO_WAIT);
        MotorRevLimit = Semaphore_pend(TiltMotorRevLimit, BIOS_NO_WAIT);

        if( !MotorFwdLimit && !MotorRevLimit )
        {
            System_printf("Tilt Motor Drive\n");
            System_flush(); /* force SysMin output to console */
        }
        else if( MotorFwdLimit )
        {
            System_printf("Tilt Motor Forward Limit Reached\n");
            System_flush(); /* force SysMin output to console */
        }
        else
        {
            System_printf("Tilt Motor Reverse Limit Reached\n");
            System_flush(); /* force SysMin output to console */
        }

        Task_sleep(10);

        System_printf("exit TiltMotorDriverTask()\n");

        System_flush(); /* force SysMin output to console */

    }
}

//-----------------------------------------
//  ======== RollMotorLimitHWI ========
//-----------------------------------------
Void RollMotorLimitHWI()
{
      GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;             // toggle ON LED9 using "TOGGLE" (1 = ON)

      if(RollMotorDirection == FWD)
          Semaphore_post(RollMotorFwdLimit); // The Forward limit of the Roll motor has been reached
      else if(RollMotorDirection == REV)
          Semaphore_post(RollMotorRevLimit); // The Reverse limit of the Roll motor has been reached
      else
      {
          System_printf("RollMotorDirection not determined\n");
          System_flush(); // force SysMin output to console
      }
}

//-----------------------------------------
//  ======== RollMotorDriverTask ========
//-----------------------------------------
Void RollMotorDriverTask()
{
    unsigned short MotorFwdLimit = 0;
    unsigned short MotorRevLimit = 0;

    while(1)
    {
        System_printf("enter RollMotorDriverTask()\n");

        MotorFwdLimit = Semaphore_pend(RollMotorFwdLimit, BIOS_NO_WAIT);
        MotorRevLimit = Semaphore_pend(RollMotorRevLimit, BIOS_NO_WAIT);

        if( !MotorFwdLimit && !MotorRevLimit )
        {
            System_printf("Roll Motor Drive\n");
            System_flush(); /* force SysMin output to console */
        }
        else if( MotorFwdLimit )
        {
            System_printf("Roll Motor Forward Limit Reached\n");
            System_flush(); /* force SysMin output to console */
        }
        else
        {
            System_printf("Roll Motor Reverse Limit Reached\n");
            System_flush(); /* force SysMin output to console */
        }

        System_printf("exit RollMotorDriverTask()\n");

        Task_sleep(10);

        System_flush(); /* force SysMin output to console */
    }
}

//-----------------------------------------
//  ======== SensorMeasurementHWI ========
//-----------------------------------------
Void SensorMeasurementHWI()
{
    unsigned int Measurement[2] = {0,0};

    SensorHighPeriod = (unsigned int)ECap1Regs.CAP2 - (unsigned int)ECap1Regs.CAP1;   // Calculate the PWM duty period (rising edge to falling edge)
    SensorPeriod = (unsigned int)ECap1Regs.CAP3 - (unsigned int)ECap1Regs.CAP1;       // Calculate the PWM period (rising edge to rising edge)

    Measurement[HIGHPERIOD] = SensorHighPeriod;
    Measurement[PERIOD] = SensorPeriod;

    Mailbox_post(SensorMeasurement, &Measurement, BIOS_NO_WAIT);

    ECap1Regs.ECCLR.bit.CEVT4 = 1;
    ECap1Regs.ECCLR.bit.INT = 1;
    ECap1Regs.ECCTL2.bit.REARM = 1;

    PieCtrlRegs.PIEACK.all = PIEACK_GROUP4;     // Acknowledge this interrupt to receive more interrupts from group 4
}

//-----------------------------------------
//  ======== SensorProcessingTask ========
//-----------------------------------------
Void SensorProcessingTask()
{
    unsigned int Measurement[2] = {0,0};
    double SensorDutyCycle = 0.0;
    double SensorAngle = 0.0;
    double HighPeriod = 0.0;
    double Period = 0.0;

    while(1)
    {
        System_printf("enter SensorProcessingTask()\n");

        Mailbox_pend(SensorMeasurement, &Measurement, BIOS_WAIT_FOREVER);
        HighPeriod = (double) Measurement[HIGHPERIOD];
        Period = (double) Measurement[PERIOD];

        SensorDutyCycle = ( HighPeriod / Period ) * 100;
        SensorDutyCycleGlobal = SensorDutyCycle;

        if( (SensorDutyCycle >= 6.0) && (SensorDutyCycle <= 94.0) )
            {
                SensorAngle = (360/88) * (SensorDutyCycle - 6.0);
                SensorAngleGlobal = SensorAngle;

                System_printf("Angle = %d deg @ DC = %d\n", SensorAngle, SensorDutyCycle);
                System_flush(); /* force SysMin output to console */
            }
        else if(SensorDutyCycle > 94.0)
            {
                System_printf("DC = %d, Fault @ >95% DC\n", SensorDutyCycle);
                System_flush(); /* force SysMin output to console */
            }
        else
            {
                System_printf("DC = %d, Fault @ <5% DC\n", SensorDutyCycle);
                System_flush(); /* force SysMin output to console */
            }

        Task_sleep(100);

        System_printf("exit SensorProcessingTask()\n");

        System_flush(); /* force SysMin output to console */

    }
}

Void TestTask()
{
//    unsigned int Measurement[2] = {0,0};

    while(1)
    {
//        System_printf("enter TestTask()\n");
//
//        SensorHighPeriod = rand();
//        SensorPeriod = 100000;
//        Measurement[HIGHPERIOD] = SensorHighPeriod;
//        Measurement[PERIOD] = SensorPeriod;
//
//        Mailbox_post(SensorMeasurement, &Measurement, BIOS_NO_WAIT);
//
//        System_printf("exit TestTask()\n");
//        System_flush(); // force SysMin output to console

        ECap1Regs.ECFRC.bit.CEVT1 = 1;
        ECap1Regs.ECFRC.bit.CEVT2 = 1;
        ECap1Regs.ECFRC.bit.CEVT3 = 1;
        ECap1Regs.ECFRC.bit.CEVT4 = 1;

        GpioDataRegs.GPATOGGLE.bit.GPIO6 = 1;             // Drive Tilt Motor output J8-pin80
        GpioDataRegs.GPATOGGLE.bit.GPIO7 = 1;             // Drive Roll Motor output J8-pin79
        Task_sleep(Period);

        if(TiltMotorDirection == FWD)
            GpioDataRegs.GPASET.bit.GPIO8 = 1;             // Set Tilt Motor direction J8-pin78
        else if(TiltMotorDirection == REV)
            GpioDataRegs.GPACLEAR.bit.GPIO8 = 1;             // Clear Tilt Motor direction J8-pin78
        else
        {
            System_printf("Stop Driving the Tilt Motor\n");
            System_flush(); // force SysMin output to console
        }

        if(RollMotorDirection == FWD)
            GpioDataRegs.GPASET.bit.GPIO9 = 1;             // Set Roll Motor direction J8-pin77
        else if(RollMotorDirection == REV)
            GpioDataRegs.GPACLEAR.bit.GPIO9 = 1;             // Clear Roll Motor direction J8-pin77
        else
        {
            System_printf("Stop Driving the Roll Motor\n");
            System_flush(); // force SysMin output to console
        }


    }
}


//-----------------------------------------
//  ======== main.c ========
//-----------------------------------------
Int main()
{
    hardware_init();                    // Intialize hardware via Xware

    System_printf("enter main()\n");

    DELAY_US(5000000L);                 // Delay for 5sec before entering the BIOS in order allow time to enumerate the USB

    BIOS_start();    /* does not return */

    return(0);
}
