Revision 34:18bcf276d3bf, committed 2015-01-07

Comitter:

pspatel321

Date:

Wed Jan 07 03:25:50 2015 +0000

Parent:

33:6bc82b6b62e5

Child:

35:9337ac9f4e1b

Commit message:

Added serial input. Updated glvBat coulomb counter to match AMS, brought in changes to outDiag and inCommands from AMS.

Changed in this revision

--- a/IOobjects/IOobjects.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/IOobjects/IOobjects.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -25,11 +25,10 @@
 Temperature internalTmp(&NXFT15XH103_TABLE, p15);    // Temperature conversion look-up table for internal temperature on the GLV bat charger FET (TABLE PTR, PIN)
 PollSwitch switches(sw, sizeof(sw)/sizeof(sw[0]));   // Shutdown switch sense pins (SWITCH PIN ARRAY, NUM PINS)
 
-CANxbee xbee1(p9,  p10, XBEE_BAUD, XBEE_TX_SIZE, XBEE_RX_SIZE);
-CANxbee xbee2(p13, p14, XBEE_BAUD, XBEE_TX_SIZE, XBEE_RX_SIZE);
-XbeeManager xbeeRelay(&xbee1, &xbee2);
+XbeeManager xbeeRelay(p9, p10, p13, p14, XBEE_BAUD, XBEE_TX_SIZE, XBEE_RX_SIZE);
 //XbeeRelay xbee;
 
 DigitalOut extras[] = {(p16), (p17)};                // Unused analog pins driven low
 Inputs data;
-CANinputs CANdata;
\ No newline at end of file
+CANinputs CANdata;
+TempData tempData;
\ No newline at end of file

--- a/IOobjects/IOobjects.h	Tue Jan 06 20:45:26 2015 +0000
+++ b/IOobjects/IOobjects.h	Wed Jan 07 03:25:50 2015 +0000
@@ -77,8 +77,13 @@
     float dcdcPump1Duty;
     float dcdcPump2Duty;
 };
+class TempData {
+public:
+    char inputStr[RX_SIZE+1];
+    char parseGoodChar;
+};
 
 extern CANinputs CANdata;
 extern Inputs data;
-
+extern TempData tempData;
 #endif
\ No newline at end of file

--- a/Libs/CANBuffer.lib	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/CANBuffer.lib	Wed Jan 07 03:25:50 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Penn-Electric-Racing/code/CANBuffer/#4baa7251c6c0
+http://developer.mbed.org/teams/Penn-Electric-Racing/code/CANBuffer/#533722a1a6cf

--- a/Libs/CoulombCounter/CoulombCounter.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/CoulombCounter/CoulombCounter.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -5,9 +5,6 @@
 const float BAT_ISENSE_OFFSET = -0.5*BAT_ISENSE_MULTIPLIER; // Offset to convert float to amps
 const float BAT_ISENSE_LIMS = 3.0;                          // Over-current limit = +/- 3A
 
-const int rtcGPREG_counter = 0;     // RTC GPREG offset for the coulomb counter
-const int rtcGPREG_capacity = 1;    // RTC GPREG offset for the capacity spec
-
 CoulombCounter::CoulombCounter(PinName _pin, int _mSec) : BatISense(_pin) {
     mSec = _mSec;
     
@@ -54,15 +51,7 @@
     tracker++;
     if (tracker >= CC_FILTER_TAPS) tracker = 0;
 }
-void CoulombCounter::resetToSOC(float SOC) { 
-    store.write(SOC*capacity(), rtcGPREG_counter);
-}
-void CoulombCounter::resetToAh(float Ah) {
-    store.write(Ah, rtcGPREG_counter);
-}
-void CoulombCounter::changeCapacity(float capAh) {
-    store.write(capAh, rtcGPREG_capacity);
-}
+
 float CoulombCounter::current() { 
     float avg = 0;
     for (int i = 0; i < CC_FILTER_TAPS; i++) {
@@ -72,15 +61,7 @@
     if (abs(avg) > BAT_ISENSE_LIMS) overCurrent = true;
     return avg;
 }
-float CoulombCounter::ampHours() {
-    return store.read(rtcGPREG_counter);
-}
-float CoulombCounter::capacity() {
-    return store.read(rtcGPREG_capacity);
-}
-float CoulombCounter::SOC() {
-    return ampHours()/capacity();
-}
+
 bool CoulombCounter::overCurrentDetected() {
-    return overCurrent;    
+    return overCurrent;
 }
\ No newline at end of file

--- a/Libs/CoulombCounter/CoulombCounter.h	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/CoulombCounter/CoulombCounter.h	Wed Jan 07 03:25:50 2015 +0000
@@ -5,31 +5,43 @@
 #include "CANBuffer.h"
 #include "RTCStore.h"
 
-const float defaultAh = 1.5;        // Default amphours of battery, in case RTC read is bad/empty
+const float defaultAh = 1.5;        // Default amphours of battery, in case store read is bad/empty
 const float defaultSOC = 0.5;
 const int CC_FILTER_TAPS = 10;
+const int rtcGPREG_counter = 0;                             // rtc GPREG offset for the coulomb counter
+const int rtcGPREG_capacity = 1;                            // rtc GPREG offset for the capacity spec
+const float MIN_CAPACITY_SETTING = 0.5;                       // Lowest allowable capacity setting
+const float MAX_CAPACITY_SETTING = 10;                        // Largest allowable capacity setting
 
 class CoulombCounter {
 public:
 
-    // Configures for a certain pin, millisecond sample period, and which GPREG in RTC to use to store the ampHours
+    // Configures for a certain pin, millisecond sample period, and which GPREG in store to use to store the ampHours
     CoulombCounter(PinName _pin, int _mSec);
     
-    // Allow zeroing the SOC when the battery is fully charged/dead, SOC in % from 0 to 1
-    void resetToSOC(float SOC);
-   
-   // Allow zeroing the SOC (via zeroing the Ah) when the battery is fully charged/dead           
-    void resetToAh(float Ah);
-    
-    // Allow change of capacity spec (changes SOC)
-    void changeCapacity(float capAh);
-    
     bool overCurrentDetected();                                 // Sensor above range
     float current();                                            // Last current reading in Amps
-    float ampHours();
-    float capacity();
-    float SOC();
     void sample();
+    
+    float capacity() { return store.read(rtcGPREG_capacity); }
+    float SOC() { return ampHours()/capacity(); }
+    float ampHours() { return store.read(rtcGPREG_counter); }
+    
+    bool changeCapacity(float _ampHours) {
+        if (_ampHours < MIN_CAPACITY_SETTING || _ampHours > MAX_CAPACITY_SETTING) return false;
+        store.write(_ampHours, rtcGPREG_capacity);
+        return true;
+    }
+    bool resetToSOC(float _SOC) {
+        if (_SOC < 0 || _SOC > 1) return false;
+        store.write(_SOC*capacity(), rtcGPREG_counter);
+        return true;
+    }
+    bool resetToAh(float _ampHours) {
+        if (_ampHours < 0 || _ampHours > capacity()) return false; 
+        store.write(_ampHours, rtcGPREG_counter);
+        return true;
+    }
 
 private:
     //Ticker sampler;         // Used to capture next sample and coulomb count

--- a/Libs/DC_DC/DC_DC.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/DC_DC/DC_DC.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -58,11 +58,8 @@
     // Do nothing if already on
     if (on && dcdcControl == ON) return;
     
-    // Do nothing if error present
-    if (critError) return;
-    
-    // If start requested
-    if (on) {
+    // If start requested and no error
+    if (on && !critError) {
         dcdcControl = ON;
         starting = true;
         startTimer.reset();
@@ -70,6 +67,7 @@
         stopTimer.stop();
         stopTimer.reset();
         stopping = false;
+        
     // If stop requested
     } else {
         stopping=true;
@@ -166,8 +164,8 @@
     if (critError || starting || stopping || !(status & 1) || dcdcControl == OFF) return;
     
     else {
-        if (chan == FAN1) fan1.write(duty);
-        if (chan == FAN2) fan2.write(duty);
+        if (chan == FAN1)  fan1.write(duty);
+        if (chan == FAN2)  fan2.write(duty);
         if (chan == PUMP1) pump1.write(duty);
         if (chan == PUMP2) pump2.write(duty);   
     }

--- a/Libs/XbeeManager/CAN-xbee/CAN-xbee.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/XbeeManager/CAN-xbee/CAN-xbee.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -1,56 +1,93 @@
 #include "CAN-xbee.h"
 
+// Creates stringified version of CANMessage msg in char* buff (must be at least 15 chars)
+// Returns -1 for bad message, or else returns size of char* buff including the terminating '\n'
+int convert2array(CANMessage &msg, char* buff) {
+    
+    // Check message integrity
+    if (msg.len > 8) return -1;
+    if ((msg.format == CANStandard) && ((msg.id & 0x7FF) != msg.id)) return -1;
+    if ((msg.format == CANExtended) && ((msg.id & 0x1FFFFFFF) != msg.id)) return -1;
+    
+    int i = 0;
+    buff[i] = 'C';      // Start of message
+    i++;
+   /* buff[i] = (msg.format << 0) | (msg.type << 1) | ((msg.len & 0xf) << 2);     // Header byte, message info on ID size, RTR, # data bytes
+    i++;
+    if (msg.format == CANStandard) {
+        buff[i++] = (msg.id & 0x0FF);               // Lower byte of ID
+        buff[i++] = (msg.id & 0x700) >> 8;          // Upper byte of ID
+    } else {
+        buff[i++] = (msg.id & 0x000000FF);          // Lowest byte of ID
+        buff[i++] = (msg.id & 0x0000FF00) >> 8;
+        buff[i++] = (msg.id & 0x00FF0000) >> 16;
+        buff[i++] = (msg.id & 0x1F000000) >> 24;    // Highest byte of ID
+    }
+    for (int j = 0; j < msg.len; j++) {
+        buff[i++] = msg.data[j];
+    }
+    buff[i++] = '\n';
+    */return i;
+}
+
+bool convert2msg(CANMessage &msg, char* buff) {
+    // Check for 'D' for done
+    if (buff[0] != 'D') return -1;
+    
+    bool extended = buff[1] & 1;
+    bool rtr = buff[1] & 2;
+    char DLC = buff[1] >> 2;
+    if (DLC > 8) return false;     // Bad length
+    
+    // Standard frame, get id and data
+    if (!extended) {
+        if (buff[3] & 0x7 != buff[3]) return false;                 // Last byte of ID bad
+        msg.id = buff[2] | (int)(buff[3] << 8);                     // Build the ID
+        for (int i = 0; i < DLC; i++) msg.data[i] = buff[i+4];      // Build the data array
+        
+    // Extended frame, get id and data
+    } else {
+        if (buff[5] & 0x1F != buff[5]) return false;                            // Last byte of ID bad
+        msg.id = buff[2] | (buff[3] << 8) | (buff[4] << 16) | (buff[5] << 24);  // Build the ID
+        for (int i = 0; i < DLC; i++) msg.data[i] = buff[i+6];                  // Build the data array
+    }
+    msg.len = DLC;
+    if (rtr) msg.type = CANRemote;
+    else msg.type = CANData;
+    if (extended) msg.format = CANExtended;
+    else msg.format = CANStandard;
+    return true;                        // Successfully parsed, passed all checks, arguement was updated
+}
+
 CANxbee::CANxbee(PinName tx, PinName rx, int _baud, int txSize, int rxSize) : serial(tx, rx, txSize, rxSize) {
     serial.baud(_baud);
     rx_i = 0;
     rxBuff[0] = 0;
-    building = false;
+    getChars = -1;
 }
 
 // Send a CAN message, first reformat it into a char array, then send over Xbees
 bool CANxbee::send(CANMessage &msg) {
-    char buff[14];      // Build the string-ified CAN message here
-    int size;
-    
-    buff[0] = 'C';
-    int i;
-    if (msg.format == 0) {              // Standard, 11-bit ID
-        buff[1] = (0 << 0) | (msg.type << 1) | ((msg.len & 0xf) << 2);
-        buff[2] = msg.id & 0xFF;        // Lower byte of ID
-        buff[3] = (msg.id & 0x7FF) >> 8;
-        for (i = 0; (i < msg.len) && (i < 8); i++) {
-            buff[i+4] = msg.data[i];    // Get data bytes
-        }
-        buff[i+4] = '\n';               // Terminate message
-        size = i+4+1;
-        
-    } else if (msg.format == 1) {       // Extended, 29-bit ID
-        buff[1] = (1 << 0) | (msg.type << 1) | ((msg.len & 0xf) << 2);
-        buff[2] = msg.id & 0x000000FF;  // Lower byte of ID
-        buff[3] = msg.id & 0x0000FF00 >> 8;
-        buff[4] = msg.id & 0x00FF0000 >> 16;
-        buff[5] = msg.id & 0x1F000000 >> 24;
-        for (i = 0; (i < msg.len) && (i < 8); i++) {
-            buff[i+6] = msg.data[i];    // Get data bytes
-        }
-        buff[i+6] = '\n';               // Terminate message
-        size = i+6+1;
-    } else return false;                // Bad message
-
+    char buff[15];                      // Build the string-ified CAN message here
+    int size = convert2array(msg, buff);
+    if (size == -1) return false;       // Bad message, string not formed
     bool success=false;
     
-    // Begin thread-safe section
-    __disable_irq();
+    size=1;
+    int bytesLeft = serial.txBufferGetSize(0) - serial.txBufferGetCount();
+    Timer t;
     
-    // Check if enough spaces in buffer
-    if ((serial.txBufferGetSize(0) - serial.txBufferGetCount()) >= size) {
+    // Begin thread-safe section
+    NVIC_DisableIRQ(TIMER3_IRQn);       // Timer3-->RTOS tick
+
+    if (bytesLeft >= size) {
         for (int i = 0; i < size; i++) serial.putc(buff[i]);    // Send the message out
         success = true;
     }
     
     // End thread-safe section
-    __enable_irq();
-    
+    NVIC_EnableIRQ(TIMER3_IRQn);       // Timer3-->RTOS tick
+
     return success;
 }
 
@@ -59,62 +96,40 @@
     int newChar = serial.getcNb();
     if (newChar == -1) return false;        // No new char
     char c = newChar & 0xFF;                // Cast to char
-    bool process=false;
     
-    char len=0;
-    // Listen for a 'C', start of new message
-    if (c == 'C' && !building) {
-        rxBuff[rx_i] = 'C';
-        rx_i++;
-        building = true;    // Now building a string
+    // Listen for a 'C', start of new message as long as not already building message
+    if (c == 'C' && (rxBuff[0] != 'C')) {
+        rx_i = 0;               // Reset to start of message
+        rxBuff[rx_i++] = 'C';   // Add the 'C', increment
         return false;
     }
-    // Keep building a new message until too big or encounter '\n'
-    if (building) {
-        rxBuff[rx_i] = c;
-        rx_i++;
-        if (c == '\n') {        // Newline! process this string
-            process = true;
-            building = false;   // Reset to keep capturing on next call
-            len = rx_i;
-            rx_i = 0;
-        }
-        else if (rx_i >= sizeof(rxBuff)) {   // Too big, bad string
-            rx_i = 0;
-            building = false;
-            return false;
+    
+    // 'C' already found, now filling in contents of message
+    if (rxBuff[0] == 'C') {
+        if (getChars == -1) {           // Get the header byte
+            rxBuff[rx_i++] = c;         // Add to string, increment
+            bool extended = c & 1;      // Is this an extended message? (4 ID bytes)
+            char DLC = c >> 2;          // How long is the data section?
+            if (DLC > 8) {              // Bad DLC, reset variables
+                getChars = -1;
+                rxBuff[0] = 0;
+                rx_i = 0;
+                return false;
+            }
+            getChars = DLC + extended?4:2;  // Need to obtain getChars more characters to complete this message
+        } else {                            // This is not a header byte, add to contents of message
+            rxBuff[rx_i++] = c;
+            getChars--;
+            if (getChars <= 0) {            // Just added the last char, done message, reset variables
+                rxBuff[0] = 'D';            // Mark 'D' for done
+                rx_i = 0;
+                getChars = -1;
+            }
         }
     }
-    if (!process) return false;
+    if (rxBuff[0] != 'D') return false;
     
     // A string is ready, process it here
-    if (rxBuff[0] != 'C') return false;     // No start char, not valid
-    
-    // Extract the data from the header byte
-    bool extended = rxBuff[1] & 1;
-    bool rtr = rxBuff[1] & 2;
-    char DLC = rxBuff[1] >> 2;
-    int id=0;
-    if (DLC > 8) return false;                  // Bad DLC
-    if (!extended) {                            // Standard ID
-        if (len > 12 || len < 5) return false;  // Too big/too small for standard size
-        if (rxBuff[3] & 0x7 != rxBuff[3]) return false;     // Last byte of ID bad
-        if (len != 5 + DLC) return false;       // Improper number of bytes
-        id = rxBuff[2] | (rxBuff[3] << 8);      // Build the ID
-        for (int i = 0; i < DLC; i++) msg.data[i] = rxBuff[i+4];        // Build the data array
-
-    } else {                                    // Extended ID
-        if (len > 14 || len < 7) return false;  // Too big/too small for extended size
-        if (rxBuff[5] & 0x1F != rxBuff[5]) return false;                                // Last byte of ID bad
-        if (len != 7 + DLC) return false;       // Improper number of bytes
-        id = rxBuff[2] | (rxBuff[3] << 8) | (rxBuff[4] << 16) | (rxBuff[5] << 24);      // Build the ID
-        for (int i = 0; i < DLC; i++) msg.data[i] = rxBuff[i+6];        // Build the data array
-    }
-    msg.id = id;
-    msg.len = DLC;
-    if (rtr) msg.type = CANRemote;
-    else msg.type = CANData;
-    if (extended) msg.format = CANExtended;
-    else msg.format = CANStandard;
-    return true;            // Successfully parsed, passed all checks, arguement was updated
+    if (convert2msg(msg, rxBuff)) return true;
+    else return false;
 }
\ No newline at end of file

--- a/Libs/XbeeManager/CAN-xbee/CAN-xbee.h	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/XbeeManager/CAN-xbee/CAN-xbee.h	Wed Jan 07 03:25:50 2015 +0000
@@ -24,9 +24,9 @@
     
 private:
     MODSERIAL serial;
-    char rxBuff[14];
+    char rxBuff[15];
     int rx_i;
-    bool building;
+    int getChars;
 };
 
 #endif
\ No newline at end of file

--- a/Libs/XbeeManager/XbeeManager.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/XbeeManager/XbeeManager.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -1,9 +1,9 @@
 #include "XbeeManager.h"
 
-XbeeManager::XbeeManager(CANxbee* _x1, CANxbee* _x2)
+XbeeManager::XbeeManager(PinName tx1, PinName rx1, PinName tx2, PinName rx2, int baud, int txSize, int rxSize) :
+                        x1(tx1, rx1, baud, txSize, rxSize), x2(tx2, rx2, baud, txSize, rxSize)
 {
-    x1 = _x1;
-    x2 = _x2;
+    
     alternate = false;
     extraAvail = false;
     counterX1in = 0;
@@ -16,12 +16,12 @@
 {
     alternate = !alternate;
     if (alternate) {
-        if (x1->send(msg)) {
+        if (x1.send(msg)) {
             counterX1out++;
             return true;   
         }
     } else {
-        if (x2->send(msg)) {
+        if (x2.send(msg)) {
             counterX2out++;
             return true;   
         }
@@ -37,14 +37,14 @@
         counterX2in++;
         return true;
     }
-    if (x1->receive(msg)) {
-        if (x2->receive(extra)) {
+    if (x1.receive(msg)) {
+        if (x2.receive(extra)) {
             extraAvail = true;
         }
         counterX1in++;
         return true;
     } else {
-        if (x2->receive(msg)) {
+        if (x2.receive(msg)) {
             counterX2in++;
             return true;   
         }

--- a/Libs/XbeeManager/XbeeManager.h	Tue Jan 06 20:45:26 2015 +0000
+++ b/Libs/XbeeManager/XbeeManager.h	Wed Jan 07 03:25:50 2015 +0000
@@ -6,7 +6,7 @@
 
 class XbeeManager {
 public:
-    XbeeManager(CANxbee* x1, CANxbee* x2);
+    XbeeManager(PinName tx1, PinName rx1, PinName tx2, PinName rx2, int baud, int txSize, int rxSize);
     bool send(CANMessage &msg);
     bool receive(CANMessage &msg); 
     unsigned int counterX1in;
@@ -16,8 +16,8 @@
     
 private:
     bool alternate;
-    CANxbee* x1;
-    CANxbee* x2;
+    CANxbee x1;
+    CANxbee x2;
     CANMessage extra;
     bool extraAvail;
 };

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inCommands/inCommands.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -0,0 +1,206 @@
+#include "inCommands.h"
+
+bool inCommands::serviceCAN(CANMessage* fromXbee)
+{
+    CANMessage msg;
+    if (fromXbee != NULL) {
+        memcpy((void*)&msg, (void*)fromXbee, sizeof(CANMessage));
+    } else {
+        if (!can.rxRead(msg)) return false;
+    }
+
+    switch (msg.id) {
+        case FAN_CONTROL_ID:
+            if (msg.len != 2*sizeof(float)) break;
+            memcpy((void*)&CANdata.dcdcFan1Duty, &msg.data[0], sizeof(float));
+            memcpy((void*)&CANdata.dcdcFan2Duty, &msg.data[4], sizeof(float));
+            dcdc.setPwm(FAN1, CANdata.dcdcFan1Duty);
+            dcdc.setPwm(FAN2, CANdata.dcdcFan2Duty);
+            break;
+
+        case PUMP_CONTROL_ID:
+            if (msg.len != 2*sizeof(float)) break;
+            memcpy((void*)&CANdata.dcdcPump1Duty, &msg.data[0], sizeof(float));
+            memcpy((void*)&CANdata.dcdcPump2Duty, &msg.data[4], sizeof(float));
+            dcdc.setPwm(PUMP1, CANdata.dcdcPump1Duty);
+            dcdc.setPwm(PUMP2, CANdata.dcdcPump2Duty);
+            break;
+
+        case DCDC_CONTROL_ID:
+            if (msg.len != sizeof(char)) break;
+            if (msg.data[0] == 1) dcdc.set(1);
+            else dcdc.set(0);
+            break;
+
+        case AMS_RELAYS_ID:
+            if (msg.len != sizeof(char)) break;
+            if ((msg.data[0] & (1<<3|1<<4|1<<5)) == (1<<3|1<<4|1<<5)) {     // AIRs closed? 1<<3=posAIR, 1<<4=negAIR, 1<<5=tractiveEnable signal
+                CANdata.airsClosed = true;
+                dcdc.set(1);
+            } else {
+                CANdata.airsClosed = false;
+                dcdc.set(0);
+            }
+            break;
+
+        case GLVBAT_CLEARSOC_ID:
+            if (msg.len != sizeof(float)) break;
+            glvBat.resetToSOC(*((float*)(&msg.data[0])));
+            break;
+
+        case GLVBAT_CLEARAH_ID:
+            if (msg.len != sizeof(float)) break;
+            glvBat.resetToAh(*((float*)(&msg.data[0])));
+            break;
+
+        case GLVBAT_SETCAPAC_ID:
+            if (msg.len != sizeof(float)) break;
+            glvBat.changeCapacity(*((float*)(&msg.data[0])));
+            break;
+        case STEERING_RESET_ID:
+            NVIC_SystemReset();
+            break;
+        default:
+            break;
+    }
+
+    return true;
+
+}
+// Check for incoming messages from the xbees, relay them to the CAN function and send them out on the bus
+bool inCommands::receiveMsgXbee()
+{
+    CANMessage msg;
+    if (xbeeRelay.receive(msg)) {                       // Incoming CAN message string received
+        if (!can.txWrite(msg)) data.canFault = true;    // Send it out on the CAN bus
+        serviceCAN(&msg);                               // Send it into the local serviceCAN routine
+        return true;
+    } else return false;
+}
+
+// Compare string to a word in the serial input, shorter to type
+#define CMP(w, string)                                              if (!strcasecmp(word[w-1], string))
+static char word[3][RX_SIZE+1];
+// Serial input
+int inCommands::serviceSerial()
+{
+
+    static int end = 0;                              // End of string position
+    int c = pc.getcNb();                             // Get char from RX buffer (returns an int)
+    if (c == -1) return -2;                          // Invalid char, no char available
+    char b = c;                                      // Casted to char type
+    bool process = false;                            // Is string complete (ready to parse)?
+
+    if (b == '\n' || b == '\r') {                    // Enter key was pressed, dump for processing
+        tempData.inputStr[end] = 0;                  // Null terminate
+        end = 0;                                     // Reset to start
+        process = true;                              // Flag for processing
+
+    } else if (b == '\b' || b == 127) {              // Backspace or delete
+        if (end > 0) end--;                          // Move back one char
+        tempData.inputStr[end] = 0;                  // Erase char
+
+    } else if (b > 31 && b < 127) {                  // New valid displayable char
+        tempData.inputStr[end] = b;                  // Add to buffer
+        end++;                                       // Increment end
+        tempData.inputStr[end] = 0;                  // Add null terminator
+        if (end >= RX_SIZE) {
+            end = 0;                                 // Reset end location
+            process = true;                          // Flag for processing
+        }
+    }
+
+    // Continue to parsing section only if flagged as complete and string not empty
+    if (!process || strlen(tempData.inputStr) == 0) return 0;
+
+    int pieces = sscanf(tempData.inputStr, "%s %s %s", word[0], word[1], word[2]);      // Populate words
+    tempData.inputStr[0] = 0;                        // Empty the string displayed on screen
+
+    char *next;                                      // Used by strtod and strtoul
+
+    // One word commands
+    if (pieces == 1) {
+        // Reset the microcontroller
+        CMP(1, "reset") {
+            NVIC_SystemReset();
+            return 1;
+        }
+        // Two word commands
+    }
+    if (pieces == 2) {
+        // Manual DC-DC on/off control
+        CMP(1, "dcdc") {
+            CMP(2, "on") {
+                dcdc.set(1);
+                return 1;
+            }
+            CMP(2, "off") {
+                dcdc.set(0);
+                return 1;
+            }
+            return -1;
+        }
+        // Artificially update the SOC (battery life %)
+        CMP(1, "SOC") {
+            CMP(2, "Reset") {                           // Command was "SOC reset" - reset to 100%, do this after a full charge
+                glvBat.resetToSOC(1);
+                return 1;
+            }
+            float soc = strtod(word[1], &next);         // Command was "SOC xxx" where xxx is float between 0 and 1
+            if (*next == 0) {
+                if (glvBat.resetToSOC(soc)) return 1;
+            }
+            return -1;
+        }
+        // Artificially update the AmpHours count (linked with SOC)
+        CMP(1, "Ah") {
+            CMP(2, "Reset") {                           // Command was "Amphours reset", equivalent to "SOC reset"
+                glvBat.resetToSOC(1);
+                return 1;
+            }
+            float ah = strtod(word[1], &next);          // Command was "Amphours xxx" where xxx is a float in amphours
+            if (*next == 0) {
+                if (glvBat.resetToAh(ah)) return 1;
+            }
+            return -1;
+        }
+        // Change the battery capacity setting for calculating Amphours
+        CMP(1, "Capacity") {
+            float cap = strtod(word[1], &next);         // Command was "SOC xxx" where xxx is float between 0 and 1
+            if (*next == 0) {
+                if (glvBat.changeCapacity(cap)) return 1;
+            }
+            return -1;   
+        }
+    }
+    if (pieces == 3) {
+        // Fan Duty
+        CMP(1, "Fan") {
+            float val1 = strtod(word[1], &next);
+            if (*next == 0) {
+                float val2 = strtod(word[2], &next);
+                if (*next == 0) {
+                    dcdc.setPwm(FAN1, val1);
+                    dcdc.setPwm(FAN2, val2);
+                    return 1;
+                }
+            }
+            return -1;
+        }
+
+        // Pump Duty
+        CMP(1, "Pump") {
+            float val1 = strtod(word[1], &next);
+            if (*next == 0) {
+                float val2 = strtod(word[2], &next);
+                if (*next == 0) {
+                    dcdc.setPwm(FAN1, val1);
+                    dcdc.setPwm(FAN2, val2);
+                    return 1;
+                }
+            }
+            return -1;
+        }
+    }
+    return -1;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inCommands/inCommands.h	Wed Jan 07 03:25:50 2015 +0000
@@ -0,0 +1,14 @@
+#ifndef _SERVICE_CAN_H
+#define _SERVICE_CAN_H
+
+#include "IOobjects.h"
+
+const int DEVICE_CAN_TIMEOUT = 0.25;      // Comms. lost to external device if a message is not received within 200 ms
+
+namespace inCommands {
+    bool serviceCAN(CANMessage* fromXbee=0);
+    bool receiveMsgXbee();
+    int serviceSerial();
+}
+
+#endif
\ No newline at end of file

--- a/main.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/main.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -1,7 +1,7 @@
 #include "IOobjects.h"
 #include "runTime.h"
 #include "outDiagnostics.h"
-#include "serviceCAN.h"
+#include "inCommands.h"
 
 int main() {
     wdt.kick();                         // Kick the watchdog timer
@@ -9,7 +9,11 @@
     pc.format(8, SerialBase::None, 2);  // 2 Stop bits, reduce bad serial packets
     
     can.mode(FIFO);
-    DigitalOut led1(LED1);
+    NVIC_SetPriority(TIMER3_IRQn, 4);
+    NVIC_SetPriority(UART0_IRQn, 0);
+    NVIC_SetPriority(CAN_IRQn, 3);
+    NVIC_SetPriority(TIMER0_IRQn, 1);
+    NVIC_SetPriority(PWM1_IRQn, 2);
     
     bool normalReset = true;
     // Did a watchdog reset occur since last power cycle?
@@ -17,7 +21,6 @@
         wdt.clearFlag();                    // Clear flag
         data.watchdogReset = true;
         pc.printf("Sys Mgmt Watchdog Reset\r\n");
-        led1=1;
         normalReset=false;
     }
     // Did a brownout reset occur since last power cycle?
@@ -30,23 +33,26 @@
     if (normalReset) pc.printf("Sys Mgmt Reset\r\n");
     
     // Start the 10Hz data thread
-    // Thread gather(runTime::thread_gather, 0, osPriorityHigh);
+    Thread gather(runTime::thread_gather, 0, osPriorityHigh, 512);
     
     // Start the 100Hz data timer (more time critical than thread)
-    //RtosTimer sample(runTime::thread_sample, osTimerPeriodic);
-    //sample.start(FAST_LOOP*1000);
+    RtosTimer sample(runTime::thread_sample, osTimerPeriodic);
+    sample.start(FAST_LOOP*1000);
     
     // Start the serial, CAN threads
-    Thread serial_out(outDiagnostics::thread_serialOut, 0, osPriorityAboveNormal, 6000);     // Allocate 6kB RAM stack
-    //Thread can_out(outDiagnostics::thread_canOut, 0, osPriorityAboveNormal);            // Allocate 256B RAM stack
+    Thread serial_out(outDiagnostics::thread_serialOut, 0, osPriorityAboveNormal, 2000);     // Allocate 6kB RAM stack
+    Thread can_out(outDiagnostics::thread_canOut, 0, osPriorityAboveNormal, 512);            // Allocate 256B RAM stack
     
-    wdt.kick(WDT_TIME);     // Startup complete, normal timeout
-    
+    wdt.kick(1);     // Startup complete, normal timeout
     // Background task
     while(1) {
         // Service CAN and Xbee logic
-        //if (canbus::serviceCAN());
-        //canbus::receiveMsgXbee();
+        inCommands::serviceCAN();
+        inCommands::receiveMsgXbee();
+        
+        int ret = inCommands::serviceSerial();
+        if (ret == -1) tempData.parseGoodChar = 'x';
+        if (ret == 1)  tempData.parseGoodChar = 251;
         wdt.kick();
     }
 }
\ No newline at end of file

--- a/outDiagnostics/outDiagnostics.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/outDiagnostics/outDiagnostics.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -1,42 +1,117 @@
 #include "outDiagnostics.h"
 
-// Macros for working with the string
-#define ADD_LINE                  len+=sprintf(buff+len,"%s\r\n",line);                                                    // Add newlines, add it to the working buffer 
-#define ADD_SPRINTF_LINE          padCenter(line,max_charsPerLine-2,temp,' '); len+=sprintf(buff+len,"%s\r\n",line);       // Add newlines, add it to the working buffer 
-#define BOOL(VAR)                 (VAR)?"ERR":"OK"
+osThreadId Serialthread_id = 0;                         // RTOS thread ID of thread_serialOut
+const int max_charsPerLine = 80;                        // Max chars per line of printed information
+const int max_lines = TX_SIZE / (max_charsPerLine+2);   // Lines per printing chunk (write chunk to MODSERIAL Buffer every x lines)
+
+// Interrupt function called when the MODSERIAL TX buffer is empty
+// The functions signals RTOS to unblock to serial thread and allow it
+// to produce the chunk of serial output data
+void empty(MODSERIAL_IRQ_INFO *q)
+{
+    osSignalSet(Serialthread_id, 1);                    // Set signal 1 for serial thread
+}
 
-// Print to a string buffer, pad to maxLen chars and center it with char pad, str must be null terminated!
-void padCenter(char *buff, int LineLen, char *str, char pad) {
-    int len = strlen(str);
-    int padL = (LineLen-len)/2;                              // -1 to save room for the null terminator
-    for (int i=0; i<padL; i++) buff[i] = pad;                // Fill in the left padding chars
-    strcpy(buff+padL, str);
-    for (int i = padL+len; i<LineLen; i++) buff[i] = pad;    // Fill remaining with padding chars
-    buff[LineLen-1] = '\0';                                  // Add null terminator
+// Add a line of text to the printing queue.
+// Actually gets added to another local buffer, which gets copied into the MODSERIAL TX buffer when it
+// empties.  Provides double-buffered output stream so inflow and outflow rates can be different.
+// Char *str NEEDS to be NULL terminated string! bool newline determines whether newline characters \r\n are appended to the line
+void AddToChunk (char *str, bool newline=true)
+{
+    static char chunk[(max_charsPerLine+5)*max_lines];          // The chunk buffer, holds one chunk= max_charsPerLine*max_lines characters
+    static int pos = 0;                                         // The position to load next character
+    static int lines = 0;                                       // Number of lines currently stored in the chunk
+    static int wait = pc.txBufferGetSize(0) * (CHAR_TIME*1000); // Max wait time to empty the tx buffer = max time to send out all chars
+
+    pos += sprintf(chunk+pos, "%s%s", str, newline?"\r\n":"");  // Append to working array
+    lines++;                                                    // New line added to array
+
+    if (lines >= max_lines) {                                   // Time to print the block
+        // Is buffer free enough to accept the block?
+        if (pc.txBufferGetCount() - pc.txBufferGetSize(0) > pos+5) {
+            for (int i = 0; i < pos; i++) pc.putc(chunk[i]);    // Add the string to the buffer
+        } else {
+            // No, it is not free enough to hold the string
+            osSignalClear(Serialthread_id, 1);                  // Clear signal if it was set
+            Thread::signal_wait(1, wait);                       // RTOS wait for empty signal, max wait if it never occurs
+            for (int i = 0; i < pos; i++) pc.putc(chunk[i]);    // Add into the output buffer
+        }
+        lines = 0;                                              // Chunk added, reset line counter
+        pos = 0;                                                // Chunk added, reset position
+    }
 }
 
-// Generates the serial dashboard, uses MODDMA MODSERIAL to speed up printing
-void outDiagnostics::thread_serialOut(void const *args) {
-    const int max_charsPerLine = 81;                 // Max chars per line
-    const int max_lines = 35;                        // Max lines that the layout prints out
+// Print to internal string buffer, pad to maxLen chars and center it with char pad, str must be null terminated!
+void padCenter(int LineLen, char *str, char pad)
+{
+    static char line[max_charsPerLine+5];                    // String buffer to work with one line at a time
+
+    int len = strlen(str);                                   // Length of input string
+    int padL = (LineLen-len)/2;                              // How many pad chars needed on left side?
+    for (int i=0; i<padL; i++) line[i] = pad;                // Fill in the left padding chars
+    strcpy(line+padL, str);                                  // Copy to line string
+    for (int i = padL+len; i<LineLen; i++) line[i] = pad;    // Fill remaining with padding chars
+    line[LineLen-1] = '\0';                                  // Add null terminator
+
+    AddToChunk(line);
+}
+
+
+// Macros for working with the strings
+#define ADD_SPRINTF_LINE    padCenter(max_charsPerLine, temp, ' ');   // Cetner the string, then add newlines, and add to chunk
+#define DIVIDER_LINE        padCenter(max_charsPerLine, "", 196);     // Generate a line full of divider char 196, add to chunk
+#define TITLE(string)       padCenter(max_charsPerLine, string, 196); // Generate a title line (centered, surrounded by char 196), add to chunk
+#define BLANK_LINE          padCenter(max_charsPerLine, "", ' ');     // Generate a line full of spaces (blank), add to chunk
+#define BOOL(VAR)           (VAR)?"ERR":"OK"
+
+// Generates the serial dashboard, uses MODSERIAL, self-paced (thread yields when buffer is full, resumes when empty)
+void outDiagnostics::thread_serialOut(void const *args)
+{
+    Serialthread_id = Thread::gettid();              // Record thread ID so empty() can signal this thread
+    char temp[max_charsPerLine+5];                   // String buffer to sprintf into, max 1 line
+
+    pc.attach(&empty, MODSERIAL::TxEmpty);           // Attach the tx empty interrupt which paces this thread
     pc.printf("\033[2J");                            // Clear the screen to get rid of reset message
 
-    char buff[max_charsPerLine*max_lines];           // Giant string to store the printout
-    char line[max_charsPerLine];                     // String buffer to work with one line at a time
-    char temp[max_charsPerLine];                     // String buffer to sprintf into
+    tempData.parseGoodChar = ' ';
+    tempData.inputStr[0] = 0;
+    
+    Timer serialLoop;                                // Timer to track the serial loop time, since this thread paces itself
+    int serialTime_ms = 0;
+    serialLoop.reset();
+    serialLoop.start();                              // Start the counter for tracking serial loop time
+
+    //const char barSpace[4] = { ' ', 179, ' ', 0 };   // Commonly used string with char 179
+
     while(1) {
 
-        int len = 0;
-        len += sprintf(buff+len, "\033[0;0H");                      // Home the cursor
-        padCenter(line, max_charsPerLine-2, "-", '-'); ADD_LINE     // Generate a line full of -'s
-        padCenter(line, max_charsPerLine-2, " Penn Electric Racing - REV0 System Management Controller Serial Dashboard ", '-'); ADD_LINE
-        padCenter(line, max_charsPerLine-2, "-", '-'); ADD_LINE     // Generate a line full of -'s
+        serialTime_ms = serialLoop.read_ms();        // Update loop timer, reset for next loop
+        serialLoop.reset();
 
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " GLV Battery ", '*'); ADD_LINE
-        sprintf(temp, "Current: %4.3fA Capacity: %4.3fAh", data.glvCurrent, data.glvCapacity); ADD_SPRINTF_LINE
-        sprintf(temp, "Amphours: %4.3fAh SOC: %5.1f%% Overcurrent: %s", data.glvAmphours, data.glvSOC*100.0, BOOL(data.glvOverCurrent)); ADD_SPRINTF_LINE
+        sprintf(temp, "\033[0;0H\033[0;0H");
+        AddToChunk(temp, false);                     // Move to 0,0, do not append newline
+        
+        DIVIDER_LINE
+        TITLE(" Penn Electric Racing - REV0 System Management Controller Dashboard ")
+        DIVIDER_LINE
         
+        int tempLength=0;
+        tempLength += sprintf(temp, "Command Input:%c %s%c", tempData.parseGoodChar, tempData.inputStr, 176);  // Command input: print header, reply, input string, and cursor marker
+        for (int i = 0; i < max_charsPerLine - strlen(temp)- 1; i++) {      // Fill in the rest of the line with blanks
+            tempLength += sprintf(temp+tempLength, " ");                    // Append spaces
+        }
+        AddToChunk(temp);        // Add this to the chunk
+        
+        TITLE(" GLV Battery ")
+        BLANK_LINE
+        sprintf(temp, "Current: %4.3fA          Capacity: %4.3fAh", data.glvCurrent, data.glvCapacity);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Amphours: %4.3fAh    SOC: %5.3f    Overcurrent: %s", data.glvAmphours, data.glvSOC, BOOL(data.glvOverCurrent));
+        ADD_SPRINTF_LINE
+
+        BLANK_LINE
+        TITLE(" DC-DC Converter ")
+        BLANK_LINE
         char DCDC = data.dcdcStatus;
         char dcdcModesStr[5][12] = {"INVALID","POWER-UP","POWER-DOWN","ON","OFF"};
         int dcdcMode = 0;
@@ -44,133 +119,120 @@
         else if (DCDC & PowerDown) dcdcMode = 2;
         else if (DCDC & SetOn)     dcdcMode = 3;
         else if (!(DCDC & SetOn))  dcdcMode = 4;
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " DC-DC Converter ", '*'); ADD_LINE
-        sprintf(temp, "Current: %5.3fA Overcurrent: %s SensorFault: %s", data.dcdcCurrent, BOOL(DCDC & OverCurrent), BOOL(DCDC & SensorFault)); ADD_SPRINTF_LINE
-        sprintf(temp, "Active: %s Mode: %s AIRS: %s StatusByte: 0x%x", (DCDC & ConvOn)?"YES":"NO", dcdcModesStr[dcdcMode], CANdata.airsClosed?"CLOSED":"OPEN", DCDC); ADD_SPRINTF_LINE
-        sprintf(temp, "StartFault: %s StopFault: %s CritErrors: %s", BOOL(DCDC & FailStart), BOOL(DCDC & FailStop), BOOL(data.dcdcError)); ADD_SPRINTF_LINE
+        sprintf(temp, "Active: %3s     Mode: %10s     AIRS: %6s     StatusByte: 0x%02x", (DCDC & ConvOn)?"YES":"NO", dcdcModesStr[dcdcMode], CANdata.airsClosed?"CLOSED":"OPEN", DCDC);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Current: %5.3fA     Overcurrent: %3s     SensorFault: %3s", data.dcdcCurrent, BOOL(DCDC & OverCurrent), BOOL(DCDC & SensorFault));
+        ADD_SPRINTF_LINE
+        sprintf(temp, "StartFault: %3s     StopFault:   %3s     CritErrors:  %3s", BOOL(DCDC & FailStart), BOOL(DCDC & FailStop), BOOL(data.dcdcError));
+        ADD_SPRINTF_LINE
 
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " PWM Channels ", '*'); ADD_LINE
-        sprintf(temp, "Actual:   FAN1: %5.1f%% FAN2: %5.1f%% PUMP1: %5.1f%% PUMP2: %5.1f%%", data.dcdcFan1Duty*100.0, data.dcdcFan2Duty*100.0, data.dcdcPump1Duty*100.0, data.dcdcPump2Duty*100.0); ADD_SPRINTF_LINE
-        sprintf(temp, "Requestd: FAN1: %5.1f%% FAN2: %5.1f%% PUMP1: %5.1f%% PUMP2: %5.1f%%", CANdata.dcdcFan1Duty*100.0, CANdata.dcdcFan2Duty*100.0, CANdata.dcdcPump1Duty*100.0, CANdata.dcdcPump2Duty*100.0); ADD_SPRINTF_LINE
+        BLANK_LINE
+        TITLE(" PWM Channels ")
+        BLANK_LINE
+        sprintf(temp, "Actual:     FAN1: %5.3f   FAN2: %5.3f   PUMP1: %5.3f   PUMP2: %5.3f", data.dcdcFan1Duty, data.dcdcFan2Duty, data.dcdcPump1Duty, data.dcdcPump2Duty);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Requestd:   FAN1: %5.3f   FAN2: %5.3f   PUMP1: %5.3f   PUMP2: %5.3f", CANdata.dcdcFan1Duty, CANdata.dcdcFan2Duty, CANdata.dcdcPump1Duty, CANdata.dcdcPump2Duty);
+        ADD_SPRINTF_LINE
 
+        BLANK_LINE
+        TITLE(" IMD ")
+        BLANK_LINE
         const char IMDstr[7][12] = {"OFF","NORMAL","UNDERVOLT","SPEEDSTART","ERROR","GROUNDFLT","INVALID"};
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " IMD ", '*'); ADD_LINE
-        sprintf(temp, "Status: %s Resistance: %7.0fkohm CritError: %s",IMDstr[data.imdStatus], data.imdResistance/1e3, BOOL(data.imdError)); ADD_SPRINTF_LINE
-        
+        sprintf(temp, "Status: %10s Resistance: %7.0fkohm CritError: %3s",IMDstr[data.imdStatus], data.imdResistance/1e3, BOOL(data.imdError));
+        ADD_SPRINTF_LINE
+
+        BLANK_LINE
+        TITLE(" Latch Circuit Monitors ")
+        BLANK_LINE
         char AMSerr = data.AMSlatchError;
         char IMDerr = data.IMDlatchError;
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " Latch Circuits ", '*'); ADD_LINE
-        sprintf(temp, "AMS: OK: %s Latch: %s SoftFault: %s HardFault: %s", (AMSerr & 1)?"LOW":"HIGH", (AMSerr & 2)?"OPEN":"OK", BOOL(AMSerr & 4), BOOL(AMSerr & 8)); ADD_SPRINTF_LINE
-        sprintf(temp, "IMD: OK: %s Latch: %s SoftFault: %s HardFault: %s", (IMDerr & 1)?"LOW":"HIGH", (IMDerr & 2)?"OPEN":"OK", BOOL(IMDerr & 4), BOOL(IMDerr & 8)); ADD_SPRINTF_LINE
+        sprintf(temp, "AMS: OK: %4s Latch: %3s SoftFault: %3s HardFault: %3s", (AMSerr & 1)?"LOW":"HIGH", BOOL(AMSerr & 2), BOOL(AMSerr & 4), BOOL(AMSerr & 8));
+        ADD_SPRINTF_LINE
+        sprintf(temp, "IMD: OK: %4s Latch: %3s SoftFault: %3s HardFault: %3s", (IMDerr & 1)?"LOW":"HIGH", BOOL(IMDerr & 2), BOOL(IMDerr & 4), BOOL(IMDerr & 8));
+        ADD_SPRINTF_LINE
 
+        BLANK_LINE
+        TITLE(" Shutdown Switches ")
+        BLANK_LINE
         char switches = data.switchState;
         const char switchNames[12][26] = {"FUSE","AMS LATCH","IMD LATCH","PCM RELAY","BRAKE PLAUSIBILITY RELAY","LEFT E-STOP","INERTIA SWITCH","BRAKE OVER-TRAVEL SWITCH","COCKPIT E-STOP","RIGHT E-STOP","HVD","TSMS"};
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line
-        padCenter(line, max_charsPerLine-2, " Shutdown Switches ", '*'); ADD_LINE
         if (switches == 0) sprintf(temp, "All switches are CLOSED.");
         else sprintf(temp, "%s is OPEN.", switchNames[switches-1]);
         ADD_SPRINTF_LINE
-        
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE   // Generate blank line
-        padCenter(line, max_charsPerLine-2, " Telemetry ", '*'); ADD_LINE
-        sprintf(temp, "Channel 1: MessagesIn: %d MessagesOut: %d", xbeeRelay.counterX1in, xbeeRelay.counterX1out); ADD_SPRINTF_LINE
-        sprintf(temp, "Channel 2: MessagesIn: %d MessagesOut: %d", xbeeRelay.counterX2in, xbeeRelay.counterX2out); ADD_SPRINTF_LINE
 
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " Miscellaneous ", '*'); ADD_LINE
-        sprintf(temp, "Temp: %5.1fC OverTemp: %s canFault: %s WatchdogReset: %s ErrorFrame: 0x%x", data.internalTemp, BOOL(data.internalOverTemp), BOOL(data.canFault), BOOL(data.watchdogReset), data.errorFrame); ADD_SPRINTF_LINE
+        BLANK_LINE
+        TITLE(" Telemetry ")
+        BLANK_LINE
+        sprintf(temp, "Channel 1: MessagesIn: %5d MessagesOut: %5d", xbeeRelay.counterX1in, xbeeRelay.counterX1out);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Channel 2: MessagesIn: %5d MessagesOut: %5d", xbeeRelay.counterX2in, xbeeRelay.counterX2out);
+        ADD_SPRINTF_LINE
 
-        // Write it all at once to output tx buffer
-        for (int i = 0; i < strlen(buff); i++) {
-            pc.putc(buff[i]);   
-        }
+        BLANK_LINE
+        TITLE(" Miscellaneous ")
+        BLANK_LINE
+        sprintf(temp, "Temp: %5.1fC %3s canFault: %3s Watchdog: %3s ErrorCode: 0x%02x SerailTime: %3dms", data.internalTemp, BOOL(data.internalOverTemp), BOOL(data.canFault), BOOL(data.watchdogReset), data.errorFrame, serialTime_ms);
+        ADD_SPRINTF_LINE
+
         // Erase screen every 5sec to remove glitches
         static int count=0;
         if (count%50==0) {
             pc.printf("\033[2J");        // Clear the screen
         }
         count++;
-        Thread::wait(100);
     }
 }
 
-template <typename T>
-void sendCAN(int ID, T dat) {
+// Macros to streamline can bus message formatting and sending
+#define SEND_CAN(LEN, ID)               msg.len=LEN; msg.id=ID; msg.type = CANData; msg.format = CANStandard; if (!can.txWrite(msg)) data.canFault = true; xbeeRelay.send(msg);
+#define FLOAT(DATA, ID)                 *((float*)((void*)&msg.data[0]))    = DATA;    SEND_CAN(sizeof(float), ID)
+#define UINT(DATA, ID)                  *((uint32_t*)((void*)&msg.data[0])) = DATA;    SEND_CAN(sizeof(uint32_t), ID)
+#define FLOAT_PAIR(DATA1, DATA2, ID)    *((float*)((void*)&msg.data[0]))    = DATA1;   *((float*)((void*)&msg.data[4]))    = DATA2;   SEND_CAN(2*sizeof(float), ID)
+#define UINT_PAIR(DATA1, DATA2, ID)     *((uint32_t*)((void*)&msg.data[0])) = DATA1;   *((uint32_t*)((void*)&msg.data[4])) = DATA2;   SEND_CAN(2*sizeof(uint32_t), ID)
+#define CHAR(DATA, ID)                  msg.data[0]                         = DATA;    SEND_CAN(sizeof(char), ID)
+
+void outDiagnostics::thread_canOut(void const *args)
+{
     CANMessage msg;
-    msg.id = ID;
-    msg.len = sizeof(T);
-    memcpy(&msg.data[0], (void*)&dat, sizeof(T));
-    if (!can.txWrite(msg)) data.canFault = true;
-    xbeeRelay.send(msg);
-}
-
-void outDiagnostics::thread_canOut(void const *args) {
-
     while(1) {
-        CANMessage msg;
 
         // Sys Mgmt Error Frame
-        sendCAN(SYS_ERROR_ID, data.errorFrame);
-        
+        CHAR(data.errorFrame, SYS_ERROR_ID)
+
         // Xbee1 Counter
-        msg.id = SYS_XBEE1_ID;
-        msg.len = 2*sizeof(int);
-        memcpy(&msg.data[0], (void*)&xbeeRelay.counterX1in, sizeof(int));
-        memcpy(&msg.data[4], (void*)&xbeeRelay.counterX1out, sizeof(int));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
+        UINT_PAIR(xbeeRelay.counterX1in, xbeeRelay.counterX1out, SYS_XBEE1_ID)
 
         // Xbee2 Counter
-        msg.id = SYS_XBEE2_ID;
-        msg.len = 2*sizeof(int);
-        memcpy(&msg.data[0], (void*)&xbeeRelay.counterX2in, sizeof(int));
-        memcpy(&msg.data[4], (void*)&xbeeRelay.counterX2out, sizeof(int));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
-        
+        UINT_PAIR(xbeeRelay.counterX2in, xbeeRelay.counterX2out, SYS_XBEE2_ID)
+
         // Internal temperature
-        sendCAN(SYS_TEMP_ID, data.internalTemp);
+        FLOAT(data.internalTemp, SYS_TEMP_ID)
 
         // GLV Battery
-        sendCAN(SYS_GLV_CURRENT_ID, data.glvCurrent);
-        sendCAN(SYS_GLV_CAPACITY_ID, data.glvCapacity);
-        sendCAN(SYS_GLV_AH_ID, data.glvAmphours);
-        sendCAN(SYS_GLV_SOC_ID, data.glvSOC);
-
+        FLOAT(data.glvCurrent, SYS_GLV_CURRENT_ID)
+        FLOAT(data.glvCapacity, SYS_GLV_CAPACITY_ID)
+        FLOAT(data.glvAmphours, SYS_GLV_AH_ID)
+        FLOAT(data.glvSOC, SYS_GLV_SOC_ID)
+        
         // DC-DC Converter
-        sendCAN(SYS_DCDC_CURRENT_ID, data.dcdcCurrent);
-        sendCAN(SYS_DCDC_STATUS_ID, data.dcdcStatus);
+        FLOAT(data.dcdcCurrent, SYS_DCDC_CURRENT_ID)
+        CHAR(data.dcdcStatus, SYS_DCDC_STATUS_ID)
 
-        // PWM Channels        
-        msg.id = SYS_PWM_FAN_ID;
-        msg.len = 2*sizeof(float);
-        memcpy(&msg.data[0], (void*)&data.dcdcFan1Duty, sizeof(float));
-        memcpy(&msg.data[4], (void*)&data.dcdcFan2Duty, sizeof(float));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
+        // PWM Channels
+        FLOAT_PAIR(data.dcdcFan1Duty, data.dcdcFan2Duty, SYS_PWM_FAN_ID)
+        FLOAT_PAIR(data.dcdcPump1Duty, data.dcdcPump2Duty, SYS_PWM_PUMP_ID)
 
-        msg.id = SYS_PWM_PUMP_ID;
-        msg.len = 2*sizeof(float);
-        memcpy(&msg.data[0], (void*)&data.dcdcPump1Duty, sizeof(float));
-        memcpy(&msg.data[4], (void*)&data.dcdcPump2Duty, sizeof(float));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
-        
         // IMD
-        sendCAN(SYS_IMD_STATUS_ID, data.imdStatus);
-        sendCAN(SYS_IMD_RESIST_ID, data.imdResistance);
+        FLOAT(data.imdStatus, SYS_IMD_STATUS_ID)
+        CHAR(data.imdResistance, SYS_IMD_RESIST_ID)
 
         // Latches
-        sendCAN(SYS_IMD_LATCH_ID, data.IMDlatchError);
-        sendCAN(SYS_AMS_LATCH_ID, data.AMSlatchError);
+        CHAR(data.IMDlatchError, SYS_IMD_LATCH_ID)
+        CHAR(data.AMSlatchError, SYS_AMS_LATCH_ID)
 
         // Shutdown Switches
-        sendCAN(SYS_SWITCHES_ID, data.switchState);
+        CHAR(data.switchState, SYS_SWITCHES_ID)
 
         Thread::wait(CAN_LOOP*1000);
-    }   
+    }
 }
\ No newline at end of file

--- a/outDiagnostics/outDiagnostics.h	Tue Jan 06 20:45:26 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#ifndef  _OUT_DIAGNOSTICS_H
-#define _OUT_DIAGNOSTICS_H
-
-#include "IOobjects.h"
-
-namespace outDiagnostics {
-    
-    void thread_canOut(void const *args);
-    void thread_serialOut(void const *args);
-    
-}
-
-#endif
\ No newline at end of file

--- a/serviceCAN/serviceCAN.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,80 +0,0 @@
-#include "serviceCAN.h"
-#include "CAN_RxIDs.h"
-
-bool canbus::serviceCAN(CANMessage* fromXbee)
-{
-    CANMessage msg;
-    if (fromXbee != NULL) {
-        memcpy((void*)&msg, (void*)fromXbee, sizeof(CANMessage));
-    } else {
-        if (!can.rxRead(msg)) return false;
-    }
-
-    switch (msg.id) {
-        case FAN_CONTROL_ID:
-            if (msg.len != 2*sizeof(float)) break;
-            memcpy((void*)&CANdata.dcdcFan1Duty, &msg.data[0], sizeof(float));
-            memcpy((void*)&CANdata.dcdcFan2Duty, &msg.data[4], sizeof(float));
-            dcdc.setPwm(FAN1, CANdata.dcdcFan1Duty);
-            dcdc.setPwm(FAN2, CANdata.dcdcFan2Duty);
-            break;
-
-        case PUMP_CONTROL_ID:
-            if (msg.len != 2*sizeof(float)) break;
-            memcpy((void*)&CANdata.dcdcPump1Duty, &msg.data[0], sizeof(float));
-            memcpy((void*)&CANdata.dcdcPump2Duty, &msg.data[4], sizeof(float));
-            dcdc.setPwm(PUMP1, CANdata.dcdcPump1Duty);
-            dcdc.setPwm(PUMP2, CANdata.dcdcPump2Duty);
-            break;
-
-        case DCDC_CONTROL_ID:
-            if (msg.len != sizeof(char)) break;
-            if (msg.data[0] == 1) dcdc.set(1);
-            else dcdc.set(0);
-            break;
-
-        case AMS_RELAYS_ID:
-            if (msg.len != sizeof(char)) break;
-            if ((msg.data[0] & (1<<3|1<<4|1<<5)) == (1<<3|1<<4|1<<5)) {     // AIRs closed? 1<<3=posAIR, 1<<4=negAIR, 1<<5=tractiveEnable signal
-                CANdata.airsClosed = true;
-                dcdc.set(1);
-            } else {
-                CANdata.airsClosed = false;
-                dcdc.set(0);
-            }
-            break;
-
-        case GLVBAT_CLEARSOC_ID:
-            if (msg.len != sizeof(float)) break;
-            glvBat.resetToSOC(*((float*)(&msg.data[0])));
-            break;
-
-        case GLVBAT_CLEARAH_ID:
-            if (msg.len != sizeof(float)) break;
-            glvBat.resetToAh(*((float*)(&msg.data[0])));
-            break;
-
-        case GLVBAT_SETCAPAC_ID:
-            if (msg.len != sizeof(float)) break;
-            glvBat.changeCapacity(*((float*)(&msg.data[0])));
-            break;
-        case STEERING_RESET_ID:
-            NVIC_SystemReset();
-            break;
-        default:
-            break;
-    }
-
-    return true;
-
-}
-// Check for incoming messages from the xbees
-bool canbus::receiveMsgXbee()
-{
-    CANMessage msg;
-    if (xbeeRelay.receive(msg)) {                       // Incoming CAN message string received
-        if (!can.txWrite(msg)) data.canFault = true;    // Send it out on the CAN bus
-        serviceCAN(&msg);                               // Send it into the local serviceCAN routine
-        return true;
-    } else return false;
-}
\ No newline at end of file

--- a/serviceCAN/serviceCAN.h	Tue Jan 06 20:45:26 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#ifndef _SERVICE_CAN_H
-#define _SERVICE_CAN_H
-
-#include "IOobjects.h"
-
-const int DEVICE_CAN_TIMEOUT = 0.25;      // Comms. lost to external device if a message is not received within 200 ms
-
-namespace canbus {
-    bool serviceCAN(CANMessage* fromXbee=0);
-    bool receiveMsgXbee();
-}
-
-#endif
\ No newline at end of file