bool InitSPECXBeeService(uint8_t Priority)
{
    ES_Event_t ThisEvent;

    MyPriority = Priority;
    // post the initial transition event
    // Configure the pins

    //First Byte, since there is nothing set to false
    DelimiterFound = false;
    
    // avoid any issues in the ISR regarding buffer size
        
    /* Set up UART2 Module */
    // Enable Interrupts and Configure Interrupts to transmit and receive
    
    // Enable input capture and global interrupts;  
    
    ThisEvent.EventType = ES_INIT;

    if (ES_PostToService(MyPriority, ThisEvent) == true)
    {
        return true;
    }
    else
    {
        return false;
    }
}


ES_Event_t RunSPECXBeeService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    if (ThisEvent.EventType == ES_INIT) {
        // Start the timer for 200 ms first so we stay on 5Hz schedule
        ES_Timer_InitTimer (MESSAGE_TIMER, 200);
    } 
    else if (ThisEvent.EventType == ES_PAIRING_REQ) {
        // Basically the mumbo jumbo to pair with the boats 
        // Update the address to transmit data to based on the boat address
    }
    else if (ThisEvent.EventType == ES_TIMEOUT) {
        // if the Message Timer timed out, then check if it is paired with the right barnacoal
        // if paired, then make the packet of data to send 
    }
    
    // Also need to handle issue with recharging
    else if (ThisEvent.EventType == ES_RECHARGE ) {
        // 
    }
 
    return ReturnEvent;
}


// Helper function to also package data into transmit buffer


void __ISR(_UART_2_VECTOR,IPL7SOFT)UART2_ISR(void) {
    __builtin_disable_interrupts();
    // Handle the Rx Buffer first ! 
    if (If the Receive Interrupt Flag is set) {
        Write the Receive register into received;
       
        Post a new event with the receive messages;
        Clear the Receive Interrupt Flag;
        
        // Do more with understanding if the bytes are correct
        // Check the start delimiter and all the bytes to follow
        // Index through the receive buffer and make sure the data is valid
        
    } 
    // handle the Tx buffer now
    else if (If Transmit Interrupts are enabled and the transmit interrupt flag is set) {
        // do some fancy stuff with the interrupt handling to make sure the
        // data is transmitted over nicely
        
        // Make sure not to clear the transmit interrupt enable mask until
        // the entire message is transmitted over
        Clear Transmit Interrupt Flag;
    }
    __builtin_enable_interrupts();
}

/****************************************************************************
 Module
   SPECXBeeService.c

 Revision
   1.0.1

 Description
   This is a template file for implementing a simple service under the
   Gen2 Events and Services Framework.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/16/12 09:58 jec      began conversion from TemplateFSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "SPECXBeeService.h"
#include <sys/attribs.h>
#include "ADPotService.h"
#include "dbprintf.h"
#include <xc.h>
#include "PWM_PIC32.h"

/*----------------------------- Module Defines ----------------------------*/
#define MAX_MESSAGE_SIZE 4
#define MAX_TX_BUF_SIZE MAX_MESSAGE_SIZE + 9
#define MAX_RX_BUF_SIZE MAX_MESSAGE_SIZE + 5
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
    static uint8_t MyPriority;
    static TemplateState_t CurrentState;
    // buffers for Tx and Rx data
    static uint8_t TX_BUF[13];
    volatile static uint8_t RX_BUF[9];

    // Tx and Rx ISR index counters to make sure the bytes being sent/received are correct
    volatile static uint8_t idx_TX;
    volatile static uint8_t idx_RX;

    // Size that a current Tx / Rx will be
    static uint8_t TxBufferSize = 13;
    volatile static uint8_t RxBufferSize;

    // Checksums sum of bytes in a Tx Message
    static uint8_t TxByteSum;
    volatile static uint8_t RxByteSum;

    // Frame ID
    static uint8_t FrameID = 0x00;

    // Boolean to track transmission of start delimiter
    volatile static bool DelimiterFound;
    
    // Union for access of bytes within the addresses
    typedef union {
        uint16_t Whole16;
        uint8_t Byte[2];
    }TwoBytes_t;
    
    static TwoBytes_t TxAddress;
    

    static TwoBytes_t RxAddress;
    
    static TwoBytes_t RxMessage;

    // Boat Addresses
    static uint16_t ADDRESSES[6] = {0x2081, 0x2082, 0x2083, 0x2084, 0x2095, 0x2086};

    // Variables determined by Comms Committee
    // Joystick 1
    static uint32_t FORWARD = 0;
    // Joystick 2
    static uint32_t TURN = 0;
    static uint8_t STATUS = 0;
    static uint8_t Buttons = 0;
    static uint8_t Charge = 0;
    
    bool IsAnchor = false;
    bool IsDump = false;
    
    uint32_t potValue = 0;
    uint32_t adjPotValue = 0;
    uint32_t JoystickX = 0;
    uint32_t JoystickY = 0;
    
    static uint8_t onesCount = 0;
    static uint8_t zerosCount = 0;
    static uint8_t readCount = 0;


/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitSPECXBeeService

 Parameters
     uint8_t : the priorty of this service

 Returns
     bool, false if error in initialization, true otherwise

 Description
     Saves away the priority, and does any
     other required initialization for this service
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitSPECXBeeService(uint8_t Priority)
{
    ES_Event_t ThisEvent;

    MyPriority = Priority;
    /* Pin Configurations */
    TRISBbits.TRISB11 = 1; // --> Configure pin for inputs Rx
    U2RXR = 0b0011;

    TRISAbits.TRISA3 = 0; //--> Configure pin for outputs Tx
    RPA3R = 0b0010;
    
    TRISAbits.TRISA0 = 1; //--> Configure pin for input for joystick
    ANSELAbits.ANSA0 = 1; // Analog
    
    TRISAbits.TRISA1 = 1; //--> Configure pin for input for joystick
    ANSELAbits.ANSA1 = 1; // Analog
    
    TRISBbits.TRISB14 = 1; //--> Configure pin for input for potentiometer
    ANSELBbits.ANSB14 = 1; // Analog
    
    TRISBbits.TRISB13 = 1; //--> Configure pin for input for potentiometer button
    ANSELBbits.ANSB13 = 0; // Digital
    
    TRISBbits.TRISB8 = 1; //--> Configure pin for input for Recharge mode button
    
    TRISAbits.TRISA0 = 1; //--> Configure pin for input for joystick
    ANSELAbits.ANSA0 = 1; // Analog
    
    TRISAbits.TRISA1 = 1; //--> Configure pin for input for joystick
    ANSELAbits.ANSA1 = 1; // Analog
    
    TRISAbits.TRISA4 = 1; //--> Configure pin for input for anchor button
    TRISBbits.TRISB5 = 1; //--> Configure pin for input for dump button
    
    TRISBbits.TRISB3 = 0; //--> Configure pin for output for battery indicator servo
    
    TRISBbits.TRISB12 = 0; //--> Configure pin for output for pairing mode indicator // RED LED
    TRISBbits.TRISB4 = 0; //--> Configure pin for output for drive mode indicator // GREEN LED
    TRISAbits.TRISA2 = 0; //--> Configure pin for output for recharge mode indicator // YELLOW LED
    ANSELBbits.ANSB12 = 0;
    LATBbits.LATB12 = 0;
    LATBbits.LATB4 = 0;
    LATAbits.LATA2 = 1;
    
    TRISBbits.TRISB10 = 1; //--> Configure pin for input for barcode scanner

    
    PWMSetup_BasicConfig(1);
    PWMSetup_AssignChannelToTimer(1, _Timer2_ );
    PWMSetup_SetPeriodOnTimer(50000, _Timer2_);
    PWMSetup_MapChannelToOutputPin(1, PWM_RPB3);
    PWMOperate_SetPulseWidthOnChannel(5000, 1);
    
    //Find the Address of the Boat that you want to send to
    // TxAddress.Whole = ADDRESSES[GET BOAT ID];
    
    //First Byte, since there is nothing set to false
    DelimiterFound = false;
    
    // avoid any issues in the ISR regarding buffer size
    
    RxBufferSize = MAX_RX_BUF_SIZE;
    
    // Hard code address
    TxAddress.Whole16 = (0x20 << 8) | 0x82;    
    /* Set up UART2 Module */
    // Disable UART2
    U2MODEbits.ON = 0;
    
    // Configure the UXMODE register to clear the SIDL, IREN, RTSMD, 
    // UEN, WAKE, LPBACK, ABAUD, RXINV
    U2MODEbits.SIDL = 0;
    U2MODEbits.IREN = 0;
    U2MODEbits.RTSMD = 1;
    U2MODEbits.WAKE = 0;
    U2MODEbits.LPBACK = 0;
    U2MODEbits.ABAUD = 0;
    U2MODEbits.UEN = 0b00;
    U2MODEbits.RXINV = 0;
    
    // Configure the UXMODE register to choose high speed or low speed baud clock
    U2MODEbits.BRGH = 0;
    
    // Configure the UXMODE register to choose the number of data bits, stop bits, and parity
    // 8 bit data, no parity
    U2MODEbits.PDSEL = 0b00;
    // one stop bit
    U2MODEbits.STSEL = 0;
    
    // Configure the UXSTA register to clear the UXINTV, UTXBRK and ADDEN bits
    U2STAbits.UTXINV = 0;
    U2STAbits.UTXBRK = 0;
    U2STAbits.ADDEN = 0;
    U2STAbits.ADM_EN = 0;
    
    U2STAbits.URXISEL = 0b00;
    U2STAbits.UTXISEL = 0b00;
    

    // Enable the receiver
    U2STAbits.URXEN = 1;
    // Enable the transmitter
    U2STAbits.UTXEN = 1;
    
    // Baud Rate of 9600
    U2BRG = 129;
    
    U2TXREG;
    U2RXREG;
    
    IFS1CLR = _IFS1_U2RXIF_MASK;
    IFS1CLR = _IFS1_U2TXIF_MASK;
    IEC1bits.U2RXIE = 1;  // Enable UART2 RX interrupt
    IEC1bits.U2TXIE = 1;
    IPC9bits.U2IP = 7;    // Set priority
    
    U2MODEbits.ON = 1;
    
    // Enable input capture and global interrupts;
    __builtin_enable_interrupts();    
    CurrentState = InitPState;
    ThisEvent.EventType = ES_INIT;

    if (ES_PostToService(MyPriority, ThisEvent) == true)
    {
        return true;
    }
    else
    {
        return false;
    }
}

/****************************************************************************
 Function
     PostSPECXBeeService

 Parameters
     EF_Event_t ThisEvent ,the event to post to the queue

 Returns
     bool false if the Enqueue operation failed, true otherwise

 Description
     Posts an event to this state machine's queue
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
bool PostSPECXBeeService(ES_Event_t ThisEvent)
{
    return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunSPECXBeeService

 Parameters
   ES_Event_t : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes

 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunSPECXBeeService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
    
    switch (CurrentState)
    {
        case InitPState: // Unpaired
        {
            if (ThisEvent.EventType == ES_INIT) {
                DB_printf("IN INIT STATE \n");
                LATBbits.LATB4 = 0;
                LATBbits.LATB12 = 1;
                LATAbits.LATA2 = 0;
                // Start the timer here once when pairing begins
                ES_Timer_InitTimer(POT_TIMER, 50);
                
            }
            else if (ThisEvent.EventType == ES_TIMEOUT) {
                // Read the potentiometer value and then print to Tera Term for debugging
                potValue = readADC();
                adjPotValue = (potValue * 100.0 / 1023);
                ES_Timer_InitTimer(POT_TIMER, 50);
                DB_printf("READING POT VALUE, %d \n", adjPotValue);
            }
            else if (ThisEvent.EventType == ES_POT_BUTTON) {
                // math to find which boat to pair with :)
                if (adjPotValue < 17) {
                    TxAddress.Whole16 = (0x20 << 8) | 0x81;   
                }
                else if (adjPotValue < 34) {
                    TxAddress.Whole16 = (0x20 << 8) | 0x82;   
                }
                else if (adjPotValue < 51) {
                    TxAddress.Whole16 = (0x20 << 8) | 0x83;   
                }
                else if (adjPotValue < 68) {
                    TxAddress.Whole16 = (0x20 << 8) | 0x84;   
                }
                else if (adjPotValue < 85) {
                    TxAddress.Whole16 = (0x20 << 8) | 0x85;   
                }
                else {
                    TxAddress.Whole16 = (0x20 << 8) | 0x86;   
                }
                DB_printf("NO LONGER READING POT VALUE, address 0x%x\n", TxAddress.Whole16);
                CurrentState = Pairing;
                ES_Timer_InitTimer(MESSAGE_TIMER, 200);
            }
        }
        break;
         
        case Pairing:
        {
            DB_printf("STATE: PAIRING\n");
            if (ThisEvent.EventType == ES_TIMEOUT) {
                // if the Message Timer timed out, then check if it is paired with the right barnacoal
                STATUS = 2;
                ConstructTx(STATUS);

                idx_TX = 0;
                IFS1SET = _IFS1_U2TXIF_MASK;
                IEC1SET = _IEC1_U2TXIE_MASK;

                // DB_printf("Sent API frame to boat at address 0x%x\n", TxAddress.Whole16);

                ES_Timer_InitTimer(MESSAGE_TIMER, 200);
            }
            
            else if (ThisEvent.EventType == ES_RX_SUCCESS) {
                // Receive buffer but not the first three bytes, so its  8-3
                if (RX_BUF[5] == 0xFF) {
                    DB_printf("Pairing Success!\n");
                    CurrentState = Paired;
                    LATAbits.LATA2 = 0;
                    LATBbits.LATB4 = 1;
                    
                    // Servo shit yurrrrrr motor go burrrrrrrr
                }
                
            }
        }
        break;
        
        case Paired:
        {
            DB_printf("STATE: PAIRED\n");
            LATBbits.LATB4 = 1;
            LATBbits.LATB12 = 0;
            LATAbits.LATA2 = 0;
//            DB_printf("This is the amount of recharge left %u\n", Charge);
            STATUS = 0;
            if (ThisEvent.EventType == ES_TIMEOUT) {
                if (ThisEvent.EventParam == MESSAGE_TIMER) {
                    // if the Message Timer timed out, then check if it is paired with the right barnacoal
                    if (Charge > 0) {
                        JoystickX = GetJoystickXValue();
                        JoystickY = GetJoystickYValue();
                        uint32_t AdjJoystickX = (uint32_t)((JoystickX * 255.0) / 1023.0 + 0.5);
                        uint32_t AdjJoystickY = (uint32_t)((JoystickY * 255.0) / 1023.0 + 0.5);
                        TURN = 255 - AdjJoystickX;
                        FORWARD = AdjJoystickY;
                        DB_printf("This is TURN %u and FORWARD %u\n", TURN, FORWARD);
//                        // ADDING A DEAD BAND ON THE SPECTACOAL
                        if ((AdjJoystickX >= 117.0) && (AdjJoystickX <= 137.0)) {
                            TURN = 127;
                        }
                        
                        if ((AdjJoystickY >= 117.0) && (AdjJoystickY <= 137.0)) {
                            FORWARD = 127;
                        }   
                    }
                    else {
                        TURN = 127;
                        FORWARD = 127;
                    }

                    ConstructTx(STATUS);
                    
                    if (IsAnchor == true || IsDump == true) {
                        Buttons = 0;
                        IsAnchor = false;
                        IsDump = false;
                    }
                    
                    idx_TX = 0;
                    IFS1SET = _IFS1_U2TXIF_MASK;
                    IEC1SET = _IEC1_U2TXIE_MASK;

                    // DB_printf("Sent API frame to boat at address 0x%x\n", TxAddress.Whole16);
                    ES_Timer_InitTimer(MESSAGE_TIMER, 200);
                }
                else if (ThisEvent.EventParam == TIMEOUT_TIMER) {
                    CurrentState = InitPState;
                    LATBbits.LATB4 = 0;
                    LATBbits.LATB12 = 1;
                    LATAbits.LATA2 = 0;
                    PWMOperate_SetPulseWidthOnChannel(5000, 1);
                }
                
                else if (ThisEvent.EventParam == BLINK_TIMER) {
                    uint8_t RedLED = PORTAbits.RA2;
                    LATAbits.LATA2 = !RedLED;
                    DB_printf("THIS SHOULD BE BLINKING\n");
                }
            }

            else if (ThisEvent.EventType == ES_RECHARGE) {
                DB_printf("This is Recharge mode\n");
                CurrentState = Recharge;
                
                // Construct recharge message or change state
            }
            
            else if (ThisEvent.EventType == ES_ANCHOR) {
                DB_printf("Anchor is being deployed\n");
                // check if the anchor is deployed first
                // bit for anchor is bit 1
                Buttons ^= (1 << 1); 
                // toggle state of isAnchor
                IsAnchor = !IsAnchor;     
                
            }
            
            else if (ThisEvent.EventType == ES_DUMP) {
                DB_printf("Dumping Coal\n");
                // check if the dump mechanism is deployed first
                // bit for dump mechanism is bit 0
                Buttons ^= (1 << 0);
                IsDump = !IsDump;
            }
            
            else if (ThisEvent.EventType == ES_RX_SUCCESS) {
                // byte 9 from BARNACOAL is the charge value
                Charge = RX_BUF[5];
                DB_printf("This is the amount of charge left %u\n", Charge);
                // Adjust the value for the LEDs to indicate charge
                // Four Second Timer for Timeout
                
                if (Charge == 0) {
                    LATAbits.LATA2 = 1;
                    ES_Timer_InitTimer(BLINK_TIMER, 2500);
                }
                PWMOperate_SetPulseWidthOnChannel(5000 - (Charge * 50 / 3) , 1);
                
                ES_Timer_InitTimer(TIMEOUT_TIMER, 4000);
            }

        }
        break;
        
        case Recharge:
        {
            
            STATUS = 1;
            LATBbits.LATB4 = 0;
            LATBbits.LATB12 = 0;
            LATAbits.LATA2 = 1;
            if (ThisEvent.EventType == ES_TIMEOUT) {
                if (ThisEvent.EventParam == MESSAGE_TIMER) {
                    // if the Message Timer timed out, then check if it is paired with the right barnacoal
                    ConstructTx(STATUS);

                    idx_TX = 0;
                    IFS1SET = _IFS1_U2TXIF_MASK;
                    IEC1SET = _IEC1_U2TXIE_MASK;

                    // DB_printf("Sent API frame to boat at address 0x%x\n", TxAddress.Whole16);

                    ES_Timer_InitTimer(MESSAGE_TIMER, 200);
                }
            }
            
            else if (ThisEvent.EventType == ES_DRIVE) {
                DB_printf("Exiting Recharge Mode\n");
                CurrentState = Paired;
                ES_Timer_InitTimer(MESSAGE_TIMER, 200);
            }
            
            else if (ThisEvent.EventType == ES_RX_SUCCESS) {
                // byte 9 from BARNACOAL is the charge value
                Charge = RX_BUF[5];
                DB_printf("STATE: RECHARGE %u \n", Charge);
                
                // PWMOperate_SetDutyOnChannel((5 + (Charge / 20)) , 1);
                PWMOperate_SetPulseWidthOnChannel(5000 - (Charge * 50 / 3) , 1);
            }

        }
        break;
    }
 
    return ReturnEvent;
}
/****************************************************************************
 Function
     QuerySPECXBeeService

 Parameters
     None

 Returns
     TemplateState_t The current state of the Template state machine

 Description
     returns the current state of the Template state machine
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:21
****************************************************************************/
TemplateState_t QuerySPECXBeeService(void)
{
  return CurrentState;
}


/***************************************************************************
 private functions
 ***************************************************************************/
bool ConstructTx (uint8_t status) {
    __builtin_disable_interrupts();
    bool returnVal = true;
    
    TX_BUF[0] = 0x7E; // Start Delimiter
    TX_BUF[1] = 0x00; // Length MSB
    TX_BUF[2] = 0x09; // Length LSB
    TX_BUF[3] = 0x01; // API identifier
    TX_BUF[4] = FrameID;
    TX_BUF[5] = TxAddress.Byte[1]; // Address MSB
    TX_BUF[6] = TxAddress.Byte[0]; // Address LSB
    TX_BUF[7] = 0x01; // Disable ACK
    TX_BUF[8] = status;
    
    // handle pairing state
    if (status == 0x02) {
        TX_BUF[9] = 0x00;
        TX_BUF[10] = 0x00;
        TX_BUF[11] = 0x00;
    }
    
    // handle driving state
    else if (status == 0x00) {
        TX_BUF[9] = FORWARD;
        TX_BUF[10] = TURN;
        TX_BUF[11] = Buttons; // This is for the button pressing, need to do logical operations
    }

    // handle Recharge state
    else if (status == 0x01) {
        TX_BUF[9] = 0x00;
        TX_BUF[10] = 0x00;
        TX_BUF[11] = 0x00;
    }
    else {
        // mishandling of data
        returnVal = false;
    }
    TxByteSum = 0;
    
    // add all the values together for checksum math
    for (int idx = 3; idx < TxBufferSize - 1; idx++) {
        TxByteSum += TX_BUF[idx];
    }
    TX_BUF[TxBufferSize - 1] = 0xFF - TxByteSum;

    __builtin_enable_interrupts();
    
    
    return returnVal;
}

void __ISR(_UART_2_VECTOR,IPL7SOFT)UART2_ISR(void) {
    __builtin_disable_interrupts();
    // Handle the Rx Buffer first ! 
    if (IFS1bits.U2RXIF) {
        uint8_t Received = U2RXREG;
        IFS1CLR = _IFS1_U2RXIF_MASK;
        if (!DelimiterFound && Received == 0x7E) {
            DelimiterFound = true;
            idx_RX = 0;
            RxByteSum = 0;
        }
        if (DelimiterFound) {
            if (idx_RX == 0) {
                // This is the start delimiter, just need to make sure it is 0x7E
            } 
            else if (idx_RX == 1) {
                RxMessage.Byte[1] = Received;
            }
            else if (idx_RX == 2) {
                RxMessage.Byte[0] = Received;
                        
                // Make sure that the length of the message is 5 bytes
                if (RxMessage.Whole16 == 0 || RxMessage.Whole16 > MAX_RX_BUF_SIZE) {
                    DelimiterFound = false;
                    idx_RX = 0;
                    RxByteSum = 0;
                }
                else {
                    RxBufferSize = RxMessage.Whole16;
                }
            }
           
            // This is the message now, so add to the Rx Byte Sum for checksum
            else if (idx_RX < (RxBufferSize + 3)) {
                RX_BUF[idx_RX - 3] = Received;
                RxByteSum += Received;
            }
            else {
                uint8_t ChecksumVal = RxByteSum + Received;
                if (ChecksumVal == 0xFF) {
                    ES_Event_t RX_EVENT;
                    RX_EVENT.EventType = ES_RX_SUCCESS;
                    RX_EVENT.EventParam = RX_BUF[0];
                    PostSPECXBeeService(RX_EVENT); 
                }
                DelimiterFound = false;
                idx_RX = 0;
                RxByteSum = 0;
            }
            idx_RX ++;
        }
    } 
    // handle the Tx buffer now
    else if (IFS1bits.U2TXIF && IEC1bits.U2TXIE) {
        U2TXREG = TX_BUF[idx_TX];  // Send next byte
            // Done sending
        idx_TX ++;
        IFS1CLR = _IFS1_U2TXIF_MASK;       // Clear TX interrupt flag
        if (TxBufferSize == idx_TX) {
            IEC1CLR = _IEC1_U2TXIE_MASK;
        }
    }   
    __builtin_enable_interrupts();
}
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/

#ifndef DriveService2_H
#define DriveService2_H

#include <stdint.h>
#include <stdbool.h>

#include "ES_Events.h"
#include "ES_Port.h"                // needed for definition of REENTRANT
// Public Function Prototypes

typedef enum {
    WaitingForCommand,
    AligningWithBeacon,
    MovingToNode,
    MovingForwardTimed,
    TurningLeft,
    TurningRight,
    TurningFull,
    LookingForBlock,
    DroppingBlock,
    TurningLeftBeacon,
    TimedReverse
} DriveServiceState_t;

bool InitDriveService2(uint8_t Priority);
bool PostDriveService2(ES_Event_t ThisEvent);
ES_Event_t RunDriveService2(ES_Event_t ThisEvent);

#endif /* ServTemplate_H */

bool Check4PairButton (void) {
	// Rising Edge Detection
	if the button is being pressed and it was not previously being pressed {
		Post an event to indicate that the SPECTACOAL should pair with BARNACOAL;
		PreviousState = CurrentState;
		return true;
	}
	PreviousState = CurrentState;
	return false;
}

bool Check4RechargeButton (void)  {

	if the Recharge button is being pressed and a rising edge is detected {
		// Reading the difference between tape and not tape
        	if the IR sensor is reading a change in signal in the barcode {
            	Post an event to indicate that the recharge requirement is being met;
            	RechargePosted = true;
                }
   	} else if the falling edge is detected {
    		Post an event to return the BARNACOAL back to drive mode;
        	Reset the onesCount, zerosCount, and readCount;
        	RechargePosted = false;
	}
PreviousButtonState = CurrentButtonState;
return false;
}

bool Check4AnchorButton (void) {
	If the button is being pressed and it was not previously being pressed {
		if it has not been milliseconds after the previous rising edge {
			LastStableState = CurrentState;
			Post an event to indicate the anchor should be deployed;
			PreviousState = CurrentState;
			return true;
		}
	}
	PreviousState = CurrentState;
	return false;
}

bool Check4DumpButton (void) {
	// Implement Debouncer
	uint32_t Now = ES_Get_Time();

	If the button is being pressed and it was not previously being pressed {
		if it has not been milliseconds after the previous rising edge {
			LastStableState = CurrentState;
			Post an event to indicate the dump mechanism should be deployed;
			PreviousState = CurrentState;
			return true;
		}
	}
	PreviousState = CurrentState;
	return false;
}

/****************************************************************************
 Module
   EventCheckers.c

 Revision
   1.0.1

 Description
   This is the sample for writing event checkers along with the event
   checkers used in the basic framework test harness.

 Notes
   Note the use of static variables in sample event checker to detect
   ONLY transitions.

 History
 When           Who     What/Why
 -------------- ---     --------
 08/06/13 13:36 jec     initial version
****************************************************************************/

// this will pull in the symbolic definitions for events, which we will want
// to post in response to detecting events
#include "ES_Configure.h"
// This gets us the prototype for ES_PostAll
#include "ES_Framework.h"
// this will get us the structure definition for events, which we will need
// in order to post events in response to detecting events
#include "ES_Events.h"
// if you want to use distribution lists then you need those function
// definitions too.
#include "ES_PostList.h"
// This include will pull in all of the headers from the service modules
// providing the prototypes for all of the post functions
#include "ES_ServiceHeaders.h"
// this test harness for the framework references the serial routines that
// are defined in ES_Port.c
#include "ES_Port.h"
// include our own prototypes to insure consistency between header &
// actual functionsdefinition
#include "EventCheckers.h"
#include "dbprintf.h"
#include "ES_Timers.h" 

// This is the event checking function sample. It is not intended to be
// included in the module. It is only here as a sample to guide you in writing
// your own event checkers
#if 0
/****************************************************************************
 Function
   Check4Lock
 Parameters
   None
 Returns
   bool: true if a new event was detected
 Description
   Sample event checker grabbed from the simple lock state machine example
 Notes
   will not compile, sample only
 Author
   J. Edward Carryer, 08/06/13, 13:48
****************************************************************************/
bool Check4Lock(void)
{
  static uint8_t  LastPinState = 0;
  uint8_t         CurrentPinState;
  bool            ReturnVal = false;

  CurrentPinState = LOCK_PIN;
  // check for pin high AND different from last time
  // do the check for difference first so that you don't bother with a test
  // of a port/variable that is not going to matter, since it hasn't changed
  if ((CurrentPinState != LastPinState) &&
      (CurrentPinState == LOCK_PIN_HI)) // event detected, so post detected event
  {
    ES_Event ThisEvent;
    ThisEvent.EventType   = ES_LOCK;
    ThisEvent.EventParam  = 1;
    // this could be any of the service post functions, ES_PostListx or
    // ES_PostAll functions
    ES_PostAll(ThisEvent);
    ReturnVal = true;
  }
  LastPinState = CurrentPinState; // update the state for next time

  return ReturnVal;
}

#endif

/****************************************************************************
 Function
   Check4Keystroke
 Parameters
   None
 Returns
   bool: true if a new key was detected & posted
 Description
   checks to see if a new key from the keyboard is detected and, if so,
   retrieves the key and posts an ES_NewKey event to TestHarnessService0
 Notes
   The functions that actually check the serial hardware for characters
   and retrieve them are assumed to be in ES_Port.c
   Since we always retrieve the keystroke when we detect it, thus clearing the
   hardware flag that indicates that a new key is ready this event checker
   will only generate events on the arrival of new characters, even though we
   do not internally keep track of the last keystroke that we retrieved.
 Author
   J. Edward Carryer, 08/06/13, 13:48
****************************************************************************/
bool Check4Keystroke(void)
{
  if (IsNewKeyReady())   // new key waiting?
  {
    ES_Event_t ThisEvent;
    ThisEvent.EventType   = ES_NEW_KEY;
    ThisEvent.EventParam  = GetNewKey();
    ES_PostAll(ThisEvent);
    return true;
  }
  return false;
}


bool Check4PairButton(void)
{
    static uint8_t PreviousState = 0;
    uint8_t CurrentState = PORTBbits.RB13;
    if ((CurrentState == 1) && (CurrentState != PreviousState)) {
        // DB_printf("Button is being pressed\n");
        // Rising edge detected
        ES_Event_t NewEvent;
        NewEvent.EventType = ES_POT_BUTTON;
        ES_PostAll(NewEvent);
        PreviousState = CurrentState; // Always update PreviousState
        return true;
    }
    
    PreviousState = CurrentState; // Always update PreviousState
    return false;
}

static uint8_t onesCount = 0;
static uint8_t zerosCount = 0;
static uint8_t readCount = 0;

bool Check4RechargeButton(void)
{
    static uint8_t PreviousButtonState = 0;
    uint8_t CurrentButtonState = PORTBbits.RB8;
    static bool RechargePosted = false;
    uint8_t BarcodeState = PORTBbits.RB10;
    
    if (CurrentButtonState == 1) {
        if (BarcodeState == 1) {
            onesCount++;
        } else {
            zerosCount++;
        }
        if (!RechargePosted && (onesCount > 30) && (onesCount <= (zerosCount + 4))) {
            DB_printf("Recharging Action\n");
            ES_Event_t NewEvent;
            NewEvent.EventType = ES_RECHARGE;
            ES_PostAll(NewEvent);
            RechargePosted = true;
        }
    }
    // Right now the issue is that even if you're holding down the button,
    // if you started the barcode scan successfully, even if you don't continue the scan it continues to recharge
    else if (CurrentButtonState == 0 && PreviousButtonState == 1) {
        ES_Event_t NewEvent;
        NewEvent.EventType = ES_DRIVE;
        ES_PostAll(NewEvent);

        onesCount = 0;
        zerosCount = 0;
        readCount = 0;
        RechargePosted = false;
    }
    
    PreviousButtonState = CurrentButtonState; // Always update PreviousState
    return false;
}

bool Check4AnchorButton(void)
{
    uint8_t CurrentState = PORTAbits.RA4;
    // Implement Debouncer
    static uint8_t LastStableState = 0;
    static uint8_t LastReadState = 0;
    static uint32_t LastDebounceTime = 0;
    uint32_t Now = ES_Timer_GetTime();
    
    if (CurrentState != LastReadState) {
        LastDebounceTime = Now;
        LastReadState = CurrentState;
    }
    if ((Now - LastDebounceTime) > 50) {
        if (LastStableState != CurrentState) {
            LastStableState = CurrentState;
            // Rising edge
            if (CurrentState == 1) {
                ES_Event_t NewEvent;
                NewEvent.EventType = ES_ANCHOR;
                ES_PostAll(NewEvent);
                return true;
            }
        }
    }
    return false;
}

bool Check4DumpButton(void)
{
    uint8_t CurrentState = PORTBbits.RB5;
    // Implement Debouncer
    static uint8_t LastStableState = 0;
    static uint8_t LastReadState = 0;
    static uint32_t LastDebounceTime = 0;
    uint32_t Now = ES_Timer_GetTime();
    
    if (CurrentState != LastReadState) {
        LastDebounceTime = Now;
        LastReadState = CurrentState;
    }
    
    if ((Now - LastDebounceTime) > 50) {
        if (LastStableState != CurrentState) {
            LastStableState = CurrentState;
        // Rising edge
            if (CurrentState == 1) {
                ES_Event_t NewEvent;
                NewEvent.EventType = ES_DUMP;
                ES_PostAll(NewEvent);
                return true;
            }
        }
    }
    return false;
}

#ifndef FollowerSPIService_H
#define FollowerSPIService_H

#include <stdint.h>
#include <stdbool.h>

#include "ES_Events.h"
#include "ES_Port.h"                // needed for definition of REENTRANT
// Public Function Prototypes

typedef enum
{
    WaitingCommand,
    CompletingAction,
    WaitingBeaconAlign,
} FollowerSPIState_t;


bool InitFollowerSPIService(uint8_t Priority);
bool PostFollowerSPIService(ES_Event_t ThisEvent);
ES_Event_t RunFollowerSPIService(ES_Event_t ThisEvent);

#endif /* ServTemplate_H */

Based on the pins mapped to be inputs for the analog settings:

Read one input pin value for the horizontal-axis joystick -- adcResults[0]
Read one input pin value for the vertical-axis joystick -- adcResults[1]
Read one input pin value for the potentiometer -- adcResults[2]


Create a helper function for reading JoystickX

Create a helper function for reading JoystickY

Create a helper function for reading Potentiometer

/****************************************************************************
 Module
   AD_PotService.c

 Revision
   1.0.1

 Description
 * Create a simple service (ADService) to handle the reading of the A/D and 
 * updating of the interval between steps. The initializer for this service 
 * would init the A/D system and start a timer (ADTimer) running to trigger 
 * speed updates. The speed updates don?t need to happen all that often. I?d 
 * suggest 50Hz (20ms interval) would be plenty fast enough. Before leaving 
 * the init function, start the timer associated with the other state machine 
 * (StepTimer, below). When the ADTimer expires, you would read the A/D and 
 * do the scaling to create the time interval to wait between steps. This A/D
 *  service ought to be implemented inside its own module, so you?ll also need
 *  an ?access function? to query the value of the scaled interval.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/16/12 09:58 jec      began conversion from TemplateFSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ADPotService.h"
#include "PIC32_AD_Lib.h"

/*----------------------------- Module Defines ----------------------------*/

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
uint32_t PotValue = 100;
uint32_t JoystickXValue = 0;  // AN0
uint32_t JoystickYValue = 0;  // AN1
uint32_t adcResults[3];



/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitTemplateService

 Parameters
     uint8_t : the priorty of this service

 Returns
     bool, false if error in initialization, true otherwise

 Description
     Saves away the priority, and does any
     other required initialization for this service
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitADService(uint8_t Priority)
{
    ES_Event_t ThisEvent;

    MyPriority = Priority;
 
    ThisEvent.EventType = ES_INIT;
  
    ANSELBbits.ANSB14 = 1;
    TRISBbits.TRISB14 = 1;
    
    ANSELAbits.ANSA0 = 1;
    TRISAbits.TRISA0 = 1;
    
    ANSELAbits.ANSA1 = 1;
    TRISAbits.TRISA1 = 1;
    
    ES_Timer_InitTimer(AD_TIMER, 20);
    // From PIC32 Datasheet, want to set the pin to be reading ADC here
    ADC_ConfigAutoScan(BIT10HI | BIT0HI | BIT1HI);

    if (ES_PostToService(MyPriority, ThisEvent) == true)
    {
        return true;
    }
    else
    {
        return false;
    }
}

/****************************************************************************
 Function
     PostTemplateService

 Parameters
     EF_Event_t ThisEvent ,the event to post to the queue

 Returns
     bool false if the Enqueue operation failed, true otherwise

 Description
     Posts an event to this state machine's queue
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
bool PostADService(ES_Event_t ThisEvent)
{
    return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunTemplateService

 Parameters
   ES_Event_t : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes

 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunADService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  
    switch (ThisEvent.EventType) {
        case ES_TIMEOUT:
            ADC_MultiRead(adcResults);
            
            JoystickXValue = adcResults[0];  // AN0 (RA0)
            JoystickYValue = adcResults[1];  // AN1 (RA1)
            PotValue = adcResults[2];
            ES_Timer_InitTimer(AD_TIMER, 20);
            break;
            
        default:
            break;
    }
    return ReturnEvent;
}

uint32_t readADC () {
    return PotValue;
}

uint32_t GetJoystickXValue(void) {
    return JoystickXValue;
}

uint32_t GetJoystickYValue(void) {
    return JoystickYValue;
}

/*------------------------------ End of file ------------------------------*/

bool InitBARNXBeeService(uint8_t Priority)
{
    ES_Event_t ThisEvent;
    MyPriority = Priority;
	Configure Input and Output pins;
    Set up UART2 module with receive and transmit interrupts;
    
    ThisEvent.EventType = ES_INIT;
    if (ES_PostToService(MyPriority, ThisEvent) == true)
    {
        return true;
    }
    else
    {
        return false;
    }
}

ES_Event_t RunBARNXBeeService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    switch(ThisEvent.EventType){
        case ES_INIT:
            
        case ES_TIMEOUT:
            if (MESSAGE_TIMER times out) {
                // Everytime the message timer times out, send a message (every 200 ms)
                Construct a message according to API framework;
                Trigger the interrupts;
            }
            
            else if (TIMEOUT_TIMER times out) {
                // if the 4 second timer runs out, there has not been a message for 4 seconds
               	Reset to unpaired state;
            }

            break;

        case MSG_RECV:
          {
            // Always start the 4 second time just in case
            if (RX_BUF[0] == 0x81) {
            	// this means that message received is formatted properly
                
                if (Status Byte is 2 (Pairing)) {
                    // Send 255 as a pairing confirmation
                    Store the Address of the received SPECTACOAL to send messages to;
                    
                    Construct a message with 0xFF (255) as the data to send;
                    Trigger the interrupt to send the message with the constructed data package

                }
                if (Status Byte is 1 (Recharging)) {
                    if (The Address is the initial address that was initially paired with) {
                        Send a package with charge status;
                        Post the Recharge Event;
                    }
                }
                if (Status Byte is 0 (Driving)) {
                    if (The Address is the initial address that was initially paired with) {
                        // drive the motors
                        Store the Joystick values;
                        Store the button values into a single byte to decode

                        Post the Driving Event
                        
                        if (The buttons have changed since the last message ) {
                    // if Buttons is 0, no anchor and no dump mechanism
                    // if Buttons is 1, deploy anchor
                    // if Buttons is 2, open dump mechanism
                    // if Buttons is 3, open dump mechanism and deploy anchor
                            Post a Button Change Event;
                        }
                    }
                }
            }
        }
          break;



        case TX_REQUESTED:
        // the barnacoal wants to send data to the spectacoal. 
        // check what kind of data it wants to send, then package it and send it over
        {
            Construct a Data Package using a helper function;
            Send the message via interrupts;
        }
        break;
        
        default:
        break;
    }
    return ReturnEvent;
}

/***************************************************************************
 private functions
 ***************************************************************************/

/////// RX and TX INTERRUPT ///////
void __ISR(_UART_2_VECTOR, IPL7SOFT) BarnISR(void)
{
    __builtin_disable_interrupts();
    
    // if we've received data, and are processing received data:
    void __ISR(_UART_2_VECTOR,IPL7SOFT)UART2_ISR(void) {
    __builtin_disable_interrupts();
    // Handle the Rx Buffer first ! 
    if (If the Receive Interrupt Flag is set) {
        Write the Receive register into received;
       
        Post a new event with the receive messages;
        Clear the Receive Interrupt Flag;
        
        // Do more with understanding if the bytes are correct
        // Check the start delimiter and all the bytes to follow
        // Index through the receive buffer and make sure the data is valid
        
    } 
    // handle the Tx buffer now
    else if (If Transmit Interrupts are enabled and the transmit interrupt flag is set) {
        // do some fancy stuff with the interrupt handling to make sure the
        // data is transmitted over nicely
        
        // Make sure not to clear the transmit interrupt enable mask until
        // the entire message is transmitted over
        Clear Transmit Interrupt Flag;
    }
    __builtin_enable_interrupts();
}

bool ConstructTx (uint8_t DataToSend) {
    bool returnVal = true;
    
    First Byte is Start Delimiter;
    Second and third Bytes are Length of Message;
    Fourth Byte is the API Identifier;
    Fifth Byte is Frame ID;
    Sixth and Seventh Bytes are Address of Device to receive message;
    Eighth Byte is set to 0x01;
    Ninth Byte is Data;
    Initialize Byte for CheckSum;
    
    Calculate the value of the Checksum so that the data values sum to 0xFF;
    
    return returnVal;
}

/****************************************************************************
 Module
   BARNXBeeService.c

 Revision
   1.0.1

 Description
   This is a template file for implementing a simple service under the
   Gen2 Events and Services Framework.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/16/12 09:58 jec      began conversion from TemplateFSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "TemplateService.h"
#include "BARNXBeeService.h"
#include "DriveService.h"
#include "BarnacoalFSM.h"

#include <sys/attribs.h>
#include "dbprintf.h"
#include <xc.h>

/*----------------------------- Module Defines ----------------------------*/
#define MAX_MESSAGE_SIZE 4
#define MAX_TX_BUF_SIZE MAX_MESSAGE_SIZE + 5
#define MAX_RX_BUF_SIZE MAX_MESSAGE_SIZE + 9

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
    static uint8_t MyPriority;


// Setup for UART transmission between XBees
    // buffers for Tx and Rx data
    static uint8_t TX_BUF[13];
    volatile static uint8_t RX_BUF[9];
    volatile static uint8_t RxData;

    // Tx and Rx ISR index counters to make sure the bytes being sent/received are correct
    volatile static uint8_t idx_TX;
    volatile static uint8_t idx_RX;

    // Size that a current Tx / Rx will be
    static uint8_t TxBufferSize = 10;
    static uint8_t RxBufferSize = 13;

    // Checksums sum of bytes in a Tx Message
    static uint8_t TxByteSum;
    volatile static uint8_t RxByteSum;

    // Frame ID
    static uint8_t FrameID;

    // Boolean to track transmission of start delimiter
    volatile static bool DelimiterFound = false;
    
    // Union for access of bytes within the addresses
    typedef union {
        uint16_t Whole16;
        uint8_t Byte[2];
    }TwoBytes_t;
    
    typedef union {
        struct {
        uint8_t Left;
        uint8_t Right;
        };
        uint16_t Whole16;
    } JoystickVals;
    
    static TwoBytes_t TxAddress;
    static TwoBytes_t RxAddress;
    static TwoBytes_t RxMessage;
    uint8_t PreviousButtons = 0;
    uint8_t Buttons = 0;
    // Boat Addresses
    static uint16_t BARN_address = 0x2082;
    
    uint8_t DataToSend = 0x00;
    uint8_t Charge = 150;


/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitBARNXBeeService

 Parameters
     uint8_t : the priorty of this service

 Returns
     bool, false if error in initialization, true otherwise

 Description
     Saves away the priority, and does any
     other required initialization for this service
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitBARNXBeeService(uint8_t Priority)
{
    ES_Event_t ThisEvent;

    MyPriority = Priority;

    PreviousButtons = 0;
    // post the initial transition event
    TRISBbits.TRISB11 = 1; // --> Configure pin B11/22 for inputs Rx, using UART2 module
    U2RXR = 0b0011; 

    TRISAbits.TRISA3 = 0; //--> Configure pin A3/10 for outputs Tx
    RPA3R = 0b0010;

    DelimiterFound = false;
    RxBufferSize = MAX_RX_BUF_SIZE;

    TRISBbits.TRISB15 = 0;    // initialize pin to drive LED when Rx buffer fills
    ANSELBbits.ANSB15 = 0;    // set B15, pin 26, to digital output
    LATBbits.LATB15 = 0;      // set it LOW

     /* Set up UART2 Module */
    // Disable UART2
    U2MODEbits.ON = 0;

    // Configure the UXMODE register to clear the SIDL, IREN, RTSMD, 
    // UEN, WAKE, LPBACK, ABAUD, RXINV
    U2MODEbits.SIDL = 0;
    U2MODEbits.IREN = 0;
    U2MODEbits.RTSMD = 1;
    U2MODEbits.WAKE = 0;
    U2MODEbits.LPBACK = 0;
    U2MODEbits.ABAUD = 0;
    U2MODEbits.UEN = 0b00;
    U2MODEbits.RXINV = 0;

    // Configure the UXMODE register to choose high speed or low speed baud clock
    U2MODEbits.BRGH = 0;

    // Configure the UXMODE register to choose the number of data bits, stop bits, and parity
    // 8 bit data, no parity
    U2MODEbits.PDSEL = 0b00;
    // one stop bit
    U2MODEbits.STSEL = 0;

    // Configure the UXSTA register to clear the UXINTV, UTXBRK and ADDEN bits
    U2STAbits.UTXINV = 0;
    U2STAbits.UTXBRK = 0;
    U2STAbits.ADDEN = 0;
    U2STAbits.ADM_EN = 0;

    U2STAbits.URXISEL = 0b00;
    U2STAbits.UTXISEL = 0b00;


    // Enable the receiver
    U2STAbits.URXEN = 1;
    // Enable the transmitter
    U2STAbits.UTXEN = 1;

    // Baud Rate of 9600
    U2BRG = 129;

    U2TXREG;
    U2RXREG;

    IFS1CLR = _IFS1_U2RXIF_MASK;
    IFS1CLR = _IFS1_U2TXIF_MASK;
    IEC1bits.U2RXIE = 1;  // Enable UART2 RX interrupt
    IEC1bits.U2TXIE = 0;  
    IPC9bits.U2IP = 7;    // Set priority

    U2MODEbits.ON = 1;

    // Enable input capture and global interrupts;
    __builtin_enable_interrupts(); 

    ThisEvent.EventType = ES_INIT;
    if (ES_PostToService(MyPriority, ThisEvent) == true)
    {
        return true;
    }
    else
    {
        return false;
    }
}

/****************************************************************************
 Function
     PostBARNXBeeService

 Parameters
     EF_Event_t ThisEvent ,the event to post to the queue

 Returns
     bool false if the Enqueue operation failed, true otherwise

 Description
     Posts an event to this state machine's queue
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
bool PostBARNXBeeService(ES_Event_t ThisEvent)
{
  return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunBARNXBeeService

 Parameters
   ES_Event_t : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes

 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunBARNXBeeService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    switch(ThisEvent.EventType){
        case ES_INIT:
            LATBbits.LATB15 = 0;
            break;
        case ES_TIMEOUT:
            if (ThisEvent.EventParam == MESSAGE_TIMER) {
                DB_printf("Data To Send %u\n", DataToSend);
                ConstructTx(DataToSend);
                idx_TX = 0;
                IFS1SET = _IFS1_U2TXIF_MASK;
                IEC1SET = _IEC1_U2TXIE_MASK;

            }
            else if (ThisEvent.EventParam == TIMEOUT_TIMER) {
                ES_Event_t NewEvent;
                NewEvent.EventType = ES_ENDOFTIME;
                PostBarnacoalFSM(NewEvent);
            }

            break;

        case MSG_RECV:
          {
            DB_printf("Message Received\n");
            ES_Timer_InitTimer(TIMEOUT_TIMER, 4000);
            if (RX_BUF[0] == 0x81) {


                // Status Byte == 2, this means that it is pairing
                if (RX_BUF[5] == 0x02) {
                    // Send 255 as a pairing confirmation
                    RxAddress.Byte[1] = RX_BUF[1];
                    RxAddress.Byte[0] = RX_BUF[2];
                    TxAddress.Whole16 = RxAddress.Whole16;
                    DB_printf("This is the Receive Address 0x%x\n", TxAddress.Whole16);

                    DataToSend = 0xFF;
                    ConstructTx(DataToSend);
                    idx_TX = 0;
                    IFS1SET = _IFS1_U2TXIF_MASK;
                    IEC1SET = _IEC1_U2TXIE_MASK;

                    ES_Event_t NewEvent;
                    NewEvent.EventType = PAIR_REQUEST;
                    ES_PostAll(NewEvent);

                }
                // Status Byte == 1, this means that it is recharging
                if (RX_BUF[5] == 0x01) {
                    if (RxAddress.Byte[1] == RX_BUF[1] && RxAddress.Byte[0] == RX_BUF[2]) {
                        // send a package with charge status
                        ES_Event_t RechargeEvent;
                        RechargeEvent.EventType = ES_RECHARGE;
                        ES_PostAll(RechargeEvent);
                    }
                }
                if (RX_BUF[5] == 0x00) {
                    if (RxAddress.Byte[1] == RX_BUF[1] && RxAddress.Byte[0] == RX_BUF[2]) {
                        // drive the motors
                        JoystickVals joystick;
                        // Left is vertical / Thrust
                        joystick.Left = RX_BUF[7];
                        // Right is Horizontal / Direction
                        joystick.Right = RX_BUF[6];
                        Buttons = RX_BUF[8];

                        ES_Event_t DriveEvent;
                        DriveEvent.EventType = ES_DRIVE;
                        DriveEvent.EventParam = joystick.Whole16;
                        ES_PostAll(DriveEvent);
                        
                        if (PreviousButtons != Buttons) {
                    // if Buttons is 0, no anchor and no dump mechanism
                    // if Buttons is 1, deploy anchor
                    // if Buttons is 2, open dump mechanism
                    // if Buttons is 3, open dump mechanism and deploy anchor
                            DB_printf("Button is being pressed\n");
                            ES_Event_t ButtonEvent;
                            ButtonEvent.EventType = ES_BUTTONCHANGE;
                            ButtonEvent.EventParam = Buttons;
                            ES_PostAll(ButtonEvent);
                            PreviousButtons = Buttons;
                        }
                    }
                }
            }
        }
          break;



        case TX_REQUESTED:
        // the barnacoal wants to send data to the spectacoal. 
        // check what kind of data it wants to send, then package it and send it over
        {
            DataToSend = ThisEvent.EventParam;
            ConstructTx(DataToSend);
            idx_TX = 0;
            IFS1SET = _IFS1_U2TXIF_MASK;
            IEC1SET = _IEC1_U2TXIE_MASK;          
//            for (int i = 0; i < TxBufferSize; i++) {
//                while (U2STAbits.UTXBF == 1);
//                DB_printf("#%d Tx_Bug %x\n",i,TX_BUF[i]);
//                U2TXREG = TX_BUF[i];
////                ES_Timer_InitTimer(MESSAGE_TIMER, 200);
//            }
        }
        break;
        
        default:
        break;
    }
    return ReturnEvent;
}

/***************************************************************************
 private functions
 ***************************************************************************/

/////// RX and TX INTERRUPT ///////
void __ISR(_UART_2_VECTOR, IPL7SOFT) BarnISR(void)
{
    __builtin_disable_interrupts();
    
    // if we've received data, and are processing received data:
    if (IFS1bits.U2RXIF && IEC1bits.U2RXIE)       // if interrupt flag has been raised
    {
        LATBbits.LATB15 = 1;         // turn on the LED for 0.5s 
        ES_Timer_InitTimer(LED_PULSE, 500);
        
        RxData = U2RXREG;             // read from Rx buffer and put into RxData
        IFS1CLR = _IFS1_U2RXIF_MASK;
        
        if (!DelimiterFound && RxData == 0x7E) {
            DelimiterFound = true;
            idx_RX = 0;
            RxByteSum = 0;
        }
        
        if (DelimiterFound) {
            if (idx_RX == 0) {
                // This is just to check that the first byte is the start delimiter
            }
            else if (idx_RX == 1) {
                RxMessage.Byte[1] = RxData;
            } 
            else if (idx_RX == 2) {
                RxMessage.Byte[0] = RxData;
                if (RxMessage.Whole16 == 0 || RxMessage.Whole16 > MAX_TX_BUF_SIZE) {
                    DelimiterFound = false;
                    idx_RX = 0;
                    RxByteSum = 0;
                } 
                else {
                    RxBufferSize = RxMessage.Whole16;
                }
            }
            
            else if (idx_RX < (RxBufferSize + 3)) {
                RX_BUF[idx_RX - 3] = RxData;
                RxByteSum += RxData;
            }
            else {
                uint8_t CheckSumVal = RxByteSum + RxData;
                if (CheckSumVal == 0xFF) {
                    ES_Event_t DataToRX;
                    DataToRX.EventType = MSG_RECV;
                    DataToRX.EventParam = RX_BUF[0];
                    PostBARNXBeeService(DataToRX);
                    
                }
                DelimiterFound = false;
                idx_RX = 0;
                RxByteSum = 0;
            }
            idx_RX ++;
        }   
    }
    
    else if (IFS1bits.U2TXIF && IEC1bits.U2TXIE) {
        
        if (idx_TX < TxBufferSize) {
            U2TXREG = TX_BUF[idx_TX];
            idx_TX ++;
            IFS1CLR = _IFS1_U2TXIF_MASK;
        }
        else {
            IEC1CLR = _IEC1_U2TXIE_MASK;
            idx_TX = 0;
        }
    }
    __builtin_enable_interrupts();
}

bool ConstructTx (uint8_t DataToSend) {
    __builtin_disable_interrupts();
    bool returnVal = true;
    
    TX_BUF[0] = 0x7E;
    TX_BUF[1] = 0x00;
    TX_BUF[2] = 0x06;
    TX_BUF[3] = 0x01;
    TX_BUF[4] = FrameID;
    TX_BUF[5] = TxAddress.Byte[1]; // Address MSB
    TX_BUF[6] = TxAddress.Byte[0]; // Address LSB
    TX_BUF[7] = 0x01;
    TX_BUF[8] = DataToSend;
    TxByteSum = 0;
    
    for (int idx = 3; idx < TxBufferSize - 1; idx++) {
        TxByteSum += TX_BUF[idx];
    }
    TX_BUF[TxBufferSize - 1] = 0xFF - TxByteSum;
    
    __builtin_enable_interrupts();
    
    return returnVal;
}

/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/

/****************************************************************************
 Template header file for Hierarchical Sate Machines AKA StateCharts

 ****************************************************************************/

#ifndef TopHSMTemplate_H
#define TopHSMTemplate_H

// State definitions for use with the query function
typedef enum { INIT_STATE, STATE_ONE, STATE_TWO, STATE_THREE, END_GAME } MasterState_t ;

// Public Function Prototypes

ES_Event_t RunMasterSM( ES_Event_t CurrentEvent );
void StartMasterSM ( ES_Event_t CurrentEvent );
bool PostMasterSM( ES_Event_t ThisEvent );
bool InitMasterSM ( uint8_t Priority );

#endif /*TopHSMTemplate_H */