Subversion Repositories Code-Repo

#include "maindefs.h"

#include "msg_queues.h"

#include "interrupts.h"

#include "uart.h"

#include "i2c.h"

#include "adc.h"

#include "timers.h"

#include "xbee.h"

#include "led_driver.h"

#include "pwm.h"

#include "delays.h"

#include "pin_interrupts.h"

#include "buffer.h"

#include "imu.h"

#include "sleep.h"

#ifdef _REMOTE

#pragma config WDTEN = OFF          // Turn off watchdog timer

#pragma config XINST = OFF          // Turn off extended instruction set

#pragma config OSC = HSPLL          // Use external oscillator (101)

#pragma config IOL1WAY = OFF        // IOLOCK bit can be set and cleared as needed

#endif

#ifdef _BASE_STATION

#pragma config WDTEN = OFF          // Turn off watchdog timer

#pragma config XINST = OFF          // Turn off extended instruction set

#pragma config OSC = INTOSCPLL      // Use internal oscillator

#pragma config IOL1WAY = OFF        // IOLOCK bit can be set and cleared as needed

#endif

/* ----------- IO Pins -----------

 * RA0 - LED Display Latch Enable (V1) or Display CLK (V2) (PPS)

 * RA1 - LED Display CLK (V1) (PPS)    or Display DIN (V2) (PPS)

 * RA2 - LED Display DIN (V1) (PPS)    or Display Latch Enable (V2)

 * RA3 - LED Display Output Enable

 * RA4 - [CANNOT BE USED (VDDCORE/VCAP)]

 * RA5 - IR Reciever (PPS)

 * RA6 - Oscillator

 * RA7 - Oscillator

 * 

 * RC0 - PWM Output (IR) (PPS, Ports B and C only)

 * RC1 - PWM Output (IR) (PPS, Ports B and C only)

 * RC2 - LED Output (PPS, Ports B and C only)

 * RC3 - I2C SCL

 * RC4 - I2C SDA

 * RC5 - XBee Sleep (PPS)

 * RC6 - UART Debug Output

 * RC7 - UART Debug Input

 *

 * RB0 - XBee CTS (PPS)

 * RB1 - XBee RTS (PPS)

 * RB2 - XBee Tx (PPS)

 * RB3 - XBee Rx (PPS)

 * RB4 - Button Input (Port B Interrupt on Change)

 * RB5 - Button Input (Port B Interrupt on Change)

 * RB6 - Button Input (Port B Interrupt on Change)

 * RB7 - Button Input (Port B Interrupt on Change)

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

#pragma udata msgbuffer

unsigned char msgbuffer[MSGLEN];

#pragma udata

void main(void) {

    XBEE_DATA xbee_data;

    I2C_DATA i2c_data;

    BUFFER_DATA buffer_data;

    int length;

    unsigned char msgtype;

    unsigned char i = 0;

#ifdef _BASE_STATION

    volatile unsigned char IR_receive_flag = 0;

    unsigned char i2c_last_req_size = 0;

    XBEE_ADDRESS_64 last_recv_address = {0};

    XBEE_ADDRESS_16 remote_address_16;

    XBEE_ADDRESS_64 remote_address_64;

#endif

#ifdef _REMOTE

    unsigned int xbee_parent_address_16 = 0;

    enum I2C_STATE i2c_state = I2C_STATE_IDLE;

    enum XBEE_STATE xbee_state = XBEE_STATE_WAITING_TO_JOIN;

    XBEE_ADDRESS_16 parent_address_16;

    XBEE_ADDRESS_64 parent_address_64;

#endif

    // Pointers to allow parsing of xbee data from arbitrary byte array

    XBEE_TX_DATA_PACKET_FRAME *frame_tx_data;

    XBEE_TX_AT_COMMAND_FRAME *frame_tx_at_command;

    XBEE_RX_AT_COMMAND_RESPONSE_FRAME *frame_at_cmd_response;

    XBEE_RX_DATA_PACKET_FRAME *frame_data_packet;

    XBEE_RX_DATA_TX_STATUS_FRAME *frame_tx_status;

    XBEE_RX_IO_DATA_SAMPLE_FRAME *frame_io_sample;

    XBEE_RX_EXPLICIT_COMMAND_FRAME *frame_explicit_cmd;

    XBEE_RX_REMOTE_AT_COMMAND_FRAME *frame_remote_at_cmd;

    XBEE_RX_ROUTE_RECORD_FRAME *frame_route_record;

    XBEE_RX_NODE_IDENTIFICATION_INDICATOR_FRAME *frame_node_identification;

    XBEE_RX_MODEM_STATUS_FRAME *frame_modem_status;

    /* --------------------- Oscillator Configuration --------------------- */

    OSCTUNEbits.PLLEN = 1;          // Enable 4x PLL

    OSCCONbits.IRCF = 0b111;        // Set INTOSC postscaler to 8MHz

    OSCCONbits.SCS = 0b00; // Use PLL as primary clock source

    OSCCONbits.IDLEN = 1;   // Set CPU to IDLE on sleep instruction

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

    // Set all ports as digial I/O

    ANCON0 = 0xFF;

    ANCON1 = 0x1F;

    uart_init(); // Initialize the UART handler code

    xbee_init(&xbee_data); // Initialize the XBee handler code

    i2c_init(&i2c_data); // Initialize the I2C handler code

    buffer_init(&buffer_data);

    //    adc_init();                 // Initialize the ADC

    MQ_init(); // Initialize message queues before enabling any interrupts

    timers_init(); // Initialize timers

    led_driver_init(); // Initialize the driver for the LED display

    port_b_int_init(); // Initialze Port B interrupt handler

#ifdef _BASE_STATION

    intx_init();    // IR receiver input

#endif

    pwm_init(); // Initialize the PWM output driver

    interrupt_enable(); // Enable high-priority interrupts and low-priority interrupts

    interrupt_init(); // Initialize the interrupt priorities

#ifdef _BASE_STATION

    i2c_configure_slave(BASE_STATION_ADDRESS);

    led_driver_num(BASE_STATION_ADDRESS);

#endif

#ifdef _REMOTE

    i2c_configure_master(); // Configure the hardware i2c device as a master

    imu_init();

#endif

    DBG_PRINT_MAIN("\r\nMain: Program Started\r\n");

#ifdef _BASE_STATION

    sleep();

#endif

#ifdef _REMOTE

    // Turn on LED until XBee is connected to network

    pwm_LED_on();

#endif

    // Loop and process recieved messages from interrupts

    while (1) {

        // Call a routine that blocks until either message queues are not empty

        MQ_wait_on_incoming_msg_queues();

        // Process high priority message queue

        length = MQ_recvmsg_ToMainFromHigh(MSGLEN, &msgtype, (void *) msgbuffer);

        if (length < 0) {

            // No message, check the error code to see if it is concern

            if (length != MSG_QUEUE_EMPTY) {

                DBG_PRINT_MAIN("Main: (ERROR) Bad high priority receive, code = %d\r\n", length);

            }

        } else {

            switch (msgtype) {

                    /* --- I2C Message Handlers ----------------------------------*/

#ifdef _BASE_STATION

                case MSGTYPE_OVERRUN:

                    DBG_PRINT_MAIN("Main: (ERROR) UART overrun detected, type = %d\r\n", msgtype);

                    break;

                case MSGTYPE_I2C_DBG:

                    DBG_PRINT_MAIN("Main: I2C Dbg Data Recieved: ");

                    for (i = 0; i < length; i++) {

                        DBG_PRINT_MAIN("%02X ", msgbuffer[i]);

                    }

                    DBG_PRINT_MAIN("\r\n");

                    break;

                case MSGTYPE_I2C_DATA:

                    DBG_PRINT_MAIN("Main: I2C Data Recieved: ");

                    for (i = 0; i < length - 1; i++) {

                        DBG_PRINT_MAIN("%02X ", msgbuffer[i]);

                    }

                    DBG_PRINT_MAIN(" Event Count: %d", msgbuffer[length - 1]);

                    DBG_PRINT_MAIN("\r\n");

                    switch (msgbuffer[0]) {

                        case 0x2:

                            length = 1;

                            // Return size of stored data in buffer

                            if (buffer_data.stored_length > MSGLEN) {

                                msgbuffer[0] = MSGLEN;

                            } else {

                                msgbuffer[0] = buffer_data.stored_length;

                            }

                            i2c_last_req_size = msgbuffer[0];

                            DBG_PRINT_MAIN("Main: (I2C Return 0x2) Returning %d\r\n", msgbuffer[0]);

                            MQ_sendmsg_FromMainToHigh(length, MSGTYPE_I2C_REPLY, (void *) msgbuffer);

                            break;

                        case 0x4:

                            // Return data stored in buffer

                            buffer_read(i2c_last_req_size, msgbuffer);

                            DBG_PRINT_MAIN("Main: (I2C Return 0x4) Returning %d bytes\r\n", i2c_last_req_size);

                            MQ_sendmsg_FromMainToHigh(i2c_last_req_size, MSGTYPE_I2C_REPLY, (void *) msgbuffer);

                            break;

                        case 0x6:

                            // Return status of IR signal input

                            length = 1;

                            msgbuffer[0] = IR_receive_flag;

                            DBG_PRINT_MAIN("Main: Returning IR state %d\r\n", IR_receive_flag);

                            MQ_sendmsg_FromMainToHigh(length, MSGTYPE_I2C_REPLY, (void *) msgbuffer);

                            break;

                        case 0x7:

                            // Send value to specified remote

                            remote_address_64.UPPER_32.char_value[0] = msgbuffer[1];

                            remote_address_64.UPPER_32.char_value[1] = msgbuffer[2];

                            remote_address_64.UPPER_32.char_value[2] = msgbuffer[3];

                            remote_address_64.UPPER_32.char_value[3] = msgbuffer[4];

                            remote_address_64.LOWER_32.char_value[0] = msgbuffer[5];

                            remote_address_64.LOWER_32.char_value[1] = msgbuffer[6];

                            remote_address_64.LOWER_32.char_value[2] = msgbuffer[7];

                            remote_address_64.LOWER_32.char_value[3] = msgbuffer[8];

                            remote_address_16.INT_16.char_value[0] = msgbuffer[9];

                            remote_address_16.INT_16.char_value[1] = msgbuffer[10];

                            DBG_PRINT_MAIN("Main: Sending %X to (64) %08lX %08lX (16) %04X\r\n",

                                    msgbuffer[11],

                                    remote_address_64.UPPER_32.long_value,

                                    remote_address_64.LOWER_32.long_value,

                                    remote_address_16.INT_16.int_value);

                            frame_tx_data = (void *) msgbuffer;

                            frame_tx_data->data[0] = msgbuffer[11];

                            frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;

                            frame_tx_data->frame_id = 0;

                            frame_tx_data->destination_64.UPPER_32.long_value = remote_address_64.UPPER_32.long_value;

                            frame_tx_data->destination_64.LOWER_32.long_value = remote_address_64.LOWER_32.long_value;

                            ConvertEndian64(&frame_tx_data->destination_64);

                            frame_tx_data->destination_16.INT_16.int_value = remote_address_16.INT_16.int_value;

                            ConvertEndian16(&frame_tx_data->destination_16);

                            frame_tx_data->broadcast_radius = 0;

                            frame_tx_data->options = 0x01; // Disable ACK

                            length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;

                            xbee_process_transmit_frame((void *) msgbuffer, length);

                            break;

                        default:

                            DBG_PRINT_MAIN("Main: (ERROR) Unexpected message type recieved: %d\r\n", msgbuffer[0]);

                            break;

                    };

                    break;

#endif

#ifdef _REMOTE

                case MSGTYPE_I2C_MASTER_SEND_COMPLETE:

                    DBG_PRINT_MAIN("Main: I2C Master Send Complete\r\n");

                    break;

                case MSGTYPE_I2C_MASTER_SEND_FAILED:

                    DBG_PRINT_MAIN("Main: (ERROR) I2C Master Send Failed\r\n");

                    break;

                case MSGTYPE_I2C_MASTER_RECV_COMPLETE:

                    DBG_PRINT_MAIN("Main: I2C Master Receive Complete\r\n");

                    DBG_PRINT_MAIN("Main: (I2C Data) ");

                    for (i = 0; i < length; i++) {

                        DBG_PRINT_MAIN("%02X ", msgbuffer[i]);

                    }

                    DBG_PRINT_MAIN("\r\n");

                    // Insert recorded value into buffer

                    if (i2c_state == I2C_STATE_READ_ACC) {

                        buffer_insert_one(RETURNID_ACC);

                        buffer_insert(6, msgbuffer);

                    } else if (i2c_state == I2C_STATE_READ_GYRO) {

                        buffer_insert_one(RETURNID_GYRO);

                        buffer_insert(6, msgbuffer);

                    }

                    if (buffer_free_space() < 7) {

                        wake();

                        // Send data to base station

                        frame_tx_data = (void *) msgbuffer;

                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;

                        frame_tx_data->frame_id = 0;

                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;

                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;

                        ConvertEndian64(&frame_tx_data->destination_64);

                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;

                        ConvertEndian16(&frame_tx_data->destination_16);

                        frame_tx_data->broadcast_radius = 0;

                        frame_tx_data->options = 0x01;  // Disable ACK

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + buffer_data.stored_length;

                        buffer_read(buffer_data.stored_length, frame_tx_data->data);

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                    }

                    break;

                case MSGTYPE_I2C_MASTER_RECV_FAILED:

                    DBG_PRINT_MAIN("Main: (ERROR) I2C Master Receive Failed\r\n");

                    break;

#endif

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

                    /* --- XBee Message Handlers ---------------------------------*/

                case MSGTYPE_XBEE_RX_AT_COMMAND_RESPONSE:

                    DBG_PRINT_MAIN("\r\n");

                    frame_at_cmd_response = (void *) msgbuffer;

                    DBG_PRINT_MAIN("Main: XBee AT command response frame\r\n");

                    DBG_PRINT_MAIN("Command: %c%c\r\n", frame_at_cmd_response->command[0], frame_at_cmd_response->command[1]);

                    DBG_PRINT_MAIN("Status: %d\r\n", frame_at_cmd_response->command_status);

                    DBG_PRINT_MAIN("Data: ");

                    for (i = 0; i < length - XBEE_RX_AT_COMMAND_RESPONSE_FRAME_SIZE; i++) {

                        DBG_PRINT_MAIN("%02X ", frame_at_cmd_response->data[i]);

                    }

                    DBG_PRINT_MAIN("\r\n");

#ifdef _REMOTE

                    // Parse response from 'MP' command

                    if (frame_at_cmd_response->command[0] == 'M' &&

                            frame_at_cmd_response->command[1] == 'P') {

                        parent_address_16.INT_16.char_value[0] = frame_at_cmd_response->data[0];

                        parent_address_16.INT_16.char_value[1] = frame_at_cmd_response->data[1];

                        ConvertEndian16(&parent_address_16);

                        xbee_parent_address_16 = parent_address_16.INT_16.int_value;

                        DBG_PRINT_MAIN("Main: XBee parent address (16): %04X\r\n", xbee_parent_address_16);

                        // Set broadcast radius to 1

                        frame_tx_at_command = (void *) msgbuffer;

                        frame_tx_at_command->frame_type = XBEE_TX_AT_COMMAND;

                        frame_tx_at_command->frame_id = 1;

                        frame_tx_at_command->command[0] = 'B';

                        frame_tx_at_command->command[1] = 'H';

                        frame_tx_at_command->data[0] = 1;

                        length = XBEE_TX_AT_COMMAND_FRAME_SIZE + 1;

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                        // Query for parent node info

                        frame_tx_at_command = (void *) msgbuffer;

                        frame_tx_at_command->frame_type = XBEE_TX_AT_COMMAND;

                        frame_tx_at_command->frame_id = 1;

                        frame_tx_at_command->command[0] = 'N';

                        frame_tx_at_command->command[1] = 'D';

                        length = XBEE_TX_AT_COMMAND_FRAME_SIZE;

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                    }

                    // Parse response from 'ND' command

                    else if (frame_at_cmd_response->command[0] == 'N' &&

                            frame_at_cmd_response->command[1] == 'D') {

                        parent_address_64.UPPER_32.char_value[0] = frame_at_cmd_response->data[2];

                        parent_address_64.UPPER_32.char_value[1] = frame_at_cmd_response->data[3];

                        parent_address_64.UPPER_32.char_value[2] = frame_at_cmd_response->data[4];

                        parent_address_64.UPPER_32.char_value[3] = frame_at_cmd_response->data[5];

                        parent_address_64.LOWER_32.char_value[0] = frame_at_cmd_response->data[6];

                        parent_address_64.LOWER_32.char_value[1] = frame_at_cmd_response->data[7];

                        parent_address_64.LOWER_32.char_value[2] = frame_at_cmd_response->data[8];

                        parent_address_64.LOWER_32.char_value[3] = frame_at_cmd_response->data[9];

                        ConvertEndian64(&parent_address_64);

                        DBG_PRINT_MAIN("Main: XBee parent address (64): %08lX %08lX\r\n",

                                parent_address_64.UPPER_32.long_value,

                                parent_address_64.LOWER_32.long_value);

                        xbee_state = XBEE_STATE_JOINED_NETWORK;

                        // Turn off LED after XBee has joined network

                        pwm_LED_off();

                    }

#endif

                    break;

                case MSGTYPE_XBEE_RX_DATA_PACKET:

                    wake();

                    DBG_PRINT_MAIN("Main: XBee data packet frame\r\n");

                    frame_data_packet = (void *) msgbuffer;

                    ConvertEndian64(&frame_data_packet->source_64);

                    ConvertEndian16(&frame_data_packet->source_16);

                    DBG_PRINT_MAIN("Source 64: %08lX-%08lX\r\n",

                            frame_data_packet->source_64.UPPER_32.long_value,

                            frame_data_packet->source_64.LOWER_32.long_value

                            );

                    DBG_PRINT_MAIN("Source 16: %04X\r\n", frame_data_packet->source_16.INT_16.int_value);

                    DBG_PRINT_MAIN("Options: 0x%02X\r\n", frame_data_packet->recieve_options);

                    DBG_PRINT_MAIN("Data: ");

                    for (i = 0; i < length - XBEE_RX_DATA_PACKET_FRAME_SIZE; i++) {

                        DBG_PRINT_MAIN("%02X ", frame_data_packet->data[i]);

                    }

                    DBG_PRINT_MAIN("\r\n");

#ifdef _BASE_STATION

                    if (frame_data_packet->source_64.LOWER_32.long_value != last_recv_address.LOWER_32.long_value ||

                            frame_data_packet->source_64.UPPER_32.long_value != last_recv_address.UPPER_32.long_value) {

                        // Save remote's 64 and 16 bit addresses

                        buffer_insert_one(RETURNID_NEW_REMOTE_SRC);

                        buffer_insert(4, frame_data_packet->source_64.UPPER_32.char_value);

                        buffer_insert(4, frame_data_packet->source_64.LOWER_32.char_value);

                        buffer_insert(2, frame_data_packet->source_16.INT_16.char_value);

                        // Save data

                        buffer_insert(length - XBEE_RX_DATA_PACKET_FRAME_SIZE, frame_data_packet->data);

                        // Save address of sender

                        last_recv_address.UPPER_32.long_value = frame_data_packet->source_64.UPPER_32.long_value;

                        last_recv_address.LOWER_32.long_value = frame_data_packet->source_64.LOWER_32.long_value;

                    } else {

                        // Receive data from same remote that previously sent data

                        buffer_insert(length - XBEE_RX_DATA_PACKET_FRAME_SIZE, frame_data_packet->data);

                    }

                    // Send value of first byte received to LED display

//                    led_driver_num(frame_data_packet->data[0]);

#endif

#ifdef _REMOTE

                    led_driver_num(frame_data_packet->data[0]);

#endif

                    sleep();

                    break;

                case MSGTYPE_XBEE_RX_DATA_TX_STATUS:

                    DBG_PRINT_MAIN("Main: XBee TX status frame\r\n");

                    frame_tx_status = (void *) msgbuffer;

                    DBG_PRINT_MAIN("Destination: %02X\r\n", frame_tx_status->destination_16);

                    DBG_PRINT_MAIN("Transmit Retry Count: %02X\r\n", frame_tx_status->transmit_retry_count);

                    DBG_PRINT_MAIN("Delivery Status: %02X\r\n", frame_tx_status->delivery_status);

                    DBG_PRINT_MAIN("Discovery Status: %02X\r\n", frame_tx_status->discovery_status);

                    break;

                case MSGTYPE_XBEE_RX_IO_DATA_SAMPLE:

                    DBG_PRINT_MAIN("Main: XBee IO data sample frame\r\n");

                    frame_io_sample = (void *) msgbuffer;

                    break;

                case MSGTYPE_XBEE_RX_EXPLICIT_COMMAND:

                    DBG_PRINT_MAIN("Main: XBee explicit command frame\r\n");

                    frame_explicit_cmd = (void *) msgbuffer;

                    break;

                case MSGTYPE_XBEE_RX_REMOTE_AT_COMMAND_RESPONSE:

                    DBG_PRINT_MAIN("Main: XBee remote AT command response frame\r\n");

                    frame_remote_at_cmd = (void *) msgbuffer;

                    break;

                case MSGTYPE_XBEE_RX_ROUTE_RECORD:

                    DBG_PRINT_MAIN("Main: XBee route record frame\r\n");

                    frame_route_record = (void *) msgbuffer;

                    break;

                case MSGTYPE_XBEE_RX_NODE_IDENTIFICATION:

                    DBG_PRINT_MAIN("Main: XBee node identification frame\r\n");

                    frame_node_identification = (void *) msgbuffer;

                    ConvertEndian64(&frame_node_identification->source_64);

                    ConvertEndian64(&frame_node_identification->remote_64);

                    ConvertEndian16(&frame_node_identification->source_16);

                    ConvertEndian16(&frame_node_identification->remote_16);

                    ConvertEndian16(&frame_node_identification->parent_16);

                    DBG_PRINT_MAIN("Source 64: %08lX-%08lX\r\n",

                            frame_node_identification->source_64.UPPER_32.long_value,

                            frame_node_identification->source_64.LOWER_32.long_value

                            );

                    DBG_PRINT_MAIN("Source 16: %04X\r\n", frame_node_identification->source_16.INT_16.int_value);

                    DBG_PRINT_MAIN("Remote 64: %08lX-%08lX\r\n",

                            frame_node_identification->remote_64.UPPER_32.long_value,

                            frame_node_identification->remote_64.LOWER_32.long_value

                            );

                    DBG_PRINT_MAIN("Remote 16: %04X\r\n", frame_node_identification->remote_16.INT_16.int_value);

                    DBG_PRINT_MAIN("Parent 16: %04X\r\n", frame_node_identification->parent_16.INT_16.int_value);

                    DBG_PRINT_MAIN("Receive Options: %02X\r\n", frame_node_identification->recieve_options);

                    buffer_insert_one(RETURNID_NEW_REMOTE_CON);

                    buffer_insert(4, frame_node_identification->remote_64.UPPER_32.char_value);

                    buffer_insert(4, frame_node_identification->remote_64.LOWER_32.char_value);

                    buffer_insert(2, frame_node_identification->remote_16.INT_16.char_value);

                    break;

                case MSGTYPE_XBEE_RX_FRAME_MODEM_STATUS:

                    DBG_PRINT_MAIN("Main: XBee modem status frame\r\n");

                    frame_modem_status = (void *) msgbuffer;

                    DBG_PRINT_MAIN("Status: %02X (", frame_modem_status->status);

                    switch(frame_modem_status->status) {

                        case 0:

                            DBG_PRINT_MAIN("Hardware Reset");

#ifdef _REMOTE

                            xbee_state = XBEE_STATE_WAITING_TO_JOIN;

#endif

                            break;

                        case 1:

                            DBG_PRINT_MAIN("Watchdog Timer Reset");

                            break;

                        case 2:

                            DBG_PRINT_MAIN("Joined Network");

#ifdef _REMOTE

                            // Query for parent's address

                            frame_tx_at_command = (void *) msgbuffer;

                            frame_tx_at_command->frame_type = XBEE_TX_AT_COMMAND;

                            frame_tx_at_command->frame_id = 1;

                            frame_tx_at_command->command[0] = 'M';

                            frame_tx_at_command->command[1] = 'P';

                            length = XBEE_TX_AT_COMMAND_FRAME_SIZE;

                            xbee_process_transmit_frame((void *) msgbuffer, length);

#endif

                            break;

                        case 3:

                            DBG_PRINT_MAIN("Disassociated");

                            pwm_LED_on();

                            break;

                        case 6:

                            DBG_PRINT_MAIN("Coordinator Started");

                            break;

                        case 7:

                            DBG_PRINT_MAIN("Network Security Key Updated");

                            break;

                        case 0x11:

                            DBG_PRINT_MAIN("Modem Config Changed While Joining");

                            break;

                    }

                    DBG_PRINT_MAIN(")\r\n");

                    break;

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

            };

//            continue;

        }

        // Process low priority queue

        length = MQ_recvmsg_ToMainFromLow(MSGLEN, &msgtype, (void *) msgbuffer);

        if (length < 0) {

            // No message, check the error code to see if it is concern

            if (length != MSG_QUEUE_EMPTY) {

                DBG_PRINT_MAIN("Main: (ERROR) Bad low priority receive, code = %d\r\n", length);

            }

        } else {

            switch (msgtype) {

                /* --- Port B Interrupt Handlers -----------------------------*/

                case MSGTYPE_PORTB_4_DOWN:

                    DBG_PRINT_MAIN("Main: Port B4 Down\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        wake(); // Wake up all components

                        timer1_enable(); // Set timer to start data polling

                        pwm_LED_on();

                    }

#endif

                    break;

                case MSGTYPE_PORTB_4_UP:

                    DBG_PRINT_MAIN("Main: Port B4 Up\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        wake();

                        timer1_disable(); // Stop data polling timer

                        // Send remaining buffer data to base station

                        frame_tx_data = (void *) msgbuffer;

                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;

                        frame_tx_data->frame_id = 0;

                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;

                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;

                        ConvertEndian64(&frame_tx_data->destination_64);

                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;

                        ConvertEndian16(&frame_tx_data->destination_16);

                        frame_tx_data->broadcast_radius = 0;

                        frame_tx_data->options = 0x01;  // Disable ACK

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + buffer_data.stored_length;

                        buffer_read(buffer_data.stored_length, frame_tx_data->data);

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                        frame_tx_data->data[0] = RETURNID_DATA_FIN;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                        i2c_state = I2C_STATE_IDLE;

                        pwm_LED_off();

                        sleep();

                    }

#endif

                    break;

                case MSGTYPE_PORTB_5_DOWN:

                    DBG_PRINT_MAIN("Main: Port B5 Down\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        wake(); // Wake up all components

                        timer3_enable(); // Enable PWM timer

                        frame_tx_data = (void *) msgbuffer;

                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;

                        frame_tx_data->frame_id = 0;

                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;

                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;

                        ConvertEndian64(&frame_tx_data->destination_64);

                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;

                        ConvertEndian16(&frame_tx_data->destination_16);

                        frame_tx_data->broadcast_radius = 0;

                        frame_tx_data->options = 0x01; // Disable ACK

                        frame_tx_data->data[0] = RETURNID_CONN;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                    }

#endif

                    break;

                case MSGTYPE_PORTB_5_UP:

                    DBG_PRINT_MAIN("Main: Port B5 Up\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        timer3_disable(); // Disable PWM timer

                        sleep();

                    }

#endif

                    break;

                case MSGTYPE_PORTB_6_DOWN:

                    DBG_PRINT_MAIN("Main: Port B6 Down\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        wake(); // Wake up all components

                        frame_tx_data = (void *) msgbuffer;

                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;

                        frame_tx_data->frame_id = 0;

                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;

                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;

                        ConvertEndian64(&frame_tx_data->destination_64);

                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;

                        ConvertEndian16(&frame_tx_data->destination_16);

                        frame_tx_data->broadcast_radius = 0;

                        frame_tx_data->options = 0x01; // Disable ACK

                        frame_tx_data->data[0] = RETURNID_BTN2;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;

                        xbee_process_transmit_frame((void *) msgbuffer, length);

                    }

#endif

                    break;

                case MSGTYPE_PORTB_6_UP:

                    DBG_PRINT_MAIN("Main: Port B6 Up\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        sleep();

                    }

#endif

                    break;

                case MSGTYPE_PORTB_7_DOWN:

                    DBG_PRINT_MAIN("Main: Port B7 Down\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        wake(); // Wake up all components

                        frame_tx_data = (void *) msgbuffer;

                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;

                        frame_tx_data->frame_id = 0;

                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;

                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;

                        ConvertEndian64(&frame_tx_data->destination_64);

                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;

                        ConvertEndian16(&frame_tx_data->destination_16);

                        frame_tx_data->broadcast_radius = 0;

                        frame_tx_data->options = 0x01; // Disable ACK

                        frame_tx_data->data[0] = RETURNID_BTN1;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;

                        xbee_process_transmit_frame((void *) msgbuffer, length);;

                    }

#endif

                    break;

                case MSGTYPE_PORTB_7_UP:

                    DBG_PRINT_MAIN("Main: Port B7 Up\r\n");

#ifdef _REMOTE

                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {

                        sleep();

                    }

#endif

                    break;

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

                case MSGTYPE_INT1:

#ifdef _BASE_STATION

                    wake();

                    IR_receive_flag = 1;

                    sleep();

//                    DBG_PRINT_MAIN("Main: INT1 Interrupt\r\n");

#endif

                    break;

                    /* --- Timer Interrupt Handlers ------------------------------*/

                case MSGTYPE_TIMER0:

                    DBG_PRINT_MAIN("Main: Timer 0 Interrupt\r\n");

#ifdef _BASE_STATION

                    IR_receive_flag = 0;    // Reset IR receive flag

                    INTCON3bits.INT1IE = 1; // Turn on INT1 interrupt

#endif

                    sleep_enable();

                    break;

                case MSGTYPE_TIMER1:

                    DBG_PRINT_MAIN("Main: Timer 1 Interrupt\r\n");

#ifdef _REMOTE

                    /* Read values from accelerometer and gyroscope */

                    if (i2c_state == I2C_STATE_READ_GYRO) {

                        imu_read_acc();

                        i2c_state = I2C_STATE_READ_ACC;

                    } else if (i2c_state == I2C_STATE_READ_ACC) {

                        imu_read_gyro();

                        i2c_state = I2C_STATE_READ_GYRO;

                    } else {    // I2C_STATE_IDLE

                        imu_read_acc();

                        i2c_state = I2C_STATE_READ_ACC;

                    }

#endif

                    break;

                case MSGTYPE_TIMER2:

                    DBG_PRINT_MAIN("Main: Timer 2 Interrupt\r\n");

#ifdef _BASE_STATION

//                    IR_receive_flag = 0;

//                    pwm_LED_off();

//                    INTCON3bits.INT1IE = 1; // Turn on INT1 interrupt

#endif

                    break;

//                case MSGTYPE_ADC_NEWVALUE:

//                    // Get the value in the ADC

//                    adc_last_value = *((unsigned int*) msgbuffer);

//                    adc_last_value_shifted = adc_last_value >> 4;

//                    DBG_PRINT_MAIN("Main: ADC Value = %d\r\n", adc_last_value);

//

//                    adc_start();

//                    break;

                default:

                    DBG_PRINT_MAIN("Main: (ERROR) Unexpected msg in low queue, length = %d, type = %d\r\n", length, msgtype);

                    for (i = 0; i < length; i++) {

                        DBG_PRINT_MAIN("%X ", msgbuffer[i]);

                    }

                    DBG_PRINT_MAIN("\r\n");

                    break;

            };

            continue;

        }

    }

}