Handheld controller (RF) for Pi Swarm system

Dependencies:   mbed

Fork of Pi_Swarm_Handheld_Controller by piswarm

Files at this revision

API Documentation at this revision

Comitter:
jah128
Date:
Tue Jun 10 11:05:23 2014 +0000
Commit message:
Handheld controller for Pi Swarm (old code)

Changed in this revision

alpha433.cpp Show annotated file Show diff for this revision Revisions of this file
alpha433.h Show annotated file Show diff for this revision Revisions of this file
communications.cpp Show annotated file Show diff for this revision Revisions of this file
communications.h Show annotated file Show diff for this revision Revisions of this file
display.cpp Show annotated file Show diff for this revision Revisions of this file
display.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r d63a63feb104 alpha433.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/alpha433.cpp	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,379 @@
+/* University of York Robot Lab m3pi Library: 433MHz Alpha Transceiver
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * October 2013
+ *
+ * Designed for use with the enhanced MBED sensor board
+ *
+ * Based on code developed by Tobias Dipper, University of Stuttgart
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+ 
+#include "mbed.h"
+#include "alpha433.h"
+#include "main.h"
+
+// Variables
+
+//Serial pc(USBTX, USBRX);
+//DigitalOut tx_led(LED2);
+//DigitalOut rx_led(LED3);
+DigitalOut irqled(LED4);
+Timeout reset_timeout;
+
+
+char cRFStatus = 0;
+
+signed short ssTransmitCount = 0;
+signed short ssTransmitPointer = 0;
+char cTXBuffer[64];
+
+signed short ssReceiveCount = 0;
+signed short ssReceivePointer = 0;
+char cRXBuffer[64];
+
+char cDataAvailable = 0;
+
+Alpha433::Alpha433(PinName mosi, PinName miso, PinName sck, PinName fss, PinName nirq) :  Stream("alpha433"), _spi(mosi,miso,sck), _fss(fss), _nirq_test(nirq), _nirq(nirq) {
+  
+}
+
+Alpha433::Alpha433() :  Stream("alpha433"), _spi(p5,p6,p7), _fss(p8), _nirq_test(p11), _nirq(p11)  {
+  
+}
+
+
+
+
+// RF Send Data
+//
+//  Eg:
+//  unsigned char message[32];
+//  unsigned char count;
+//  count = snprintf(message, 32, "Hello: %i", 42);
+//  sendString(count, message);
+unsigned long Alpha433::sendString(char cCount, char* cBuffer)
+{
+    //pc.printf("SendString called");
+    char i = 0;
+    if(cRFStatus == ALPHA433_MODE_TRANSMITTING) {// RF already transmitting
+            pc.printf("Error: Already transmitting\n");
+
+        return 1; // Error
+        
+        }
+  
+    if(cCount > 62) {// Amount of data to high
+               pc.printf("Error: Too much tx data\n");
+
+        return 2; // Error
+  
+    }
+    if(cCount == 0) {// No Data
+               pc.printf("Error: No tx data\n");
+        return 3; // Error
+    }
+    cTXBuffer[i] = cCount;
+
+    unsigned char checksum_byte = 0;  
+    for(i=0; i<cCount; i++)   {// make a copy
+        cTXBuffer[i+1] = cBuffer[i];
+        checksum_byte ^= cBuffer[i];
+    }    
+    cTXBuffer[cCount+1] = checksum_byte;
+    //pc.printf("Message: \"%s\" Checksum: %2X\n",cBuffer,checksum_byte);
+    ssTransmitCount = cCount+3; // add count and checksum
+    ssTransmitPointer = -6;
+    cRFStatus = ALPHA433_MODE_SWITCHING;
+    disableReceiver();
+    enableTransmitter();
+    cRFStatus = ALPHA433_MODE_TRANSMITTING; 
+    
+    //pc.printf("Transmitting %d bytes\n",ssTransmitCount);
+
+    while(ssTransmitPointer <= ssTransmitCount){
+       while(_nirq_test);
+       
+       if(ssTransmitPointer < -2)
+                    _write(0xB8AA);  // send sync
+                else if(ssTransmitPointer == -2)
+                    _write(0xB82D);  // send first part of the fifo pattern;
+                else if(ssTransmitPointer == -1)
+                    _write(0xB8D4);  // send second part of the fifo pattern;
+                else if(ssTransmitPointer == ssTransmitCount)
+                    _write(0xB800);   // send dummy byte
+                else 
+                     _write(0xB800 | cTXBuffer[ssTransmitPointer]);   // send data
+      ssTransmitPointer++; 
+    }
+                              
+    _write(0xB800);   // send dummy byte, maybe redundant
+    disableTransmitter();
+    enableReceiver();
+    ssReceivePointer = 0;
+    cRFStatus = ALPHA433_MODE_RECEIVING;
+    return 0;
+}
+
+// Enable RF Transmitter
+void Alpha433::enableTransmitter(void)
+{
+    //pc.printf("Enable TX\n");
+    //RFCommand(0x8229);
+    _write(0x8229);
+    //tx_led = 1;
+}
+
+// Disable RF Transmitter
+void Alpha433::disableTransmitter(void)
+{
+    //pc.printf("Disable TX\n");
+    //RFCommand(0x8209);
+    _write(0x8209);
+   // tx_led = 0;
+
+}
+
+
+// Enable RF Receiver
+void Alpha433::enableReceiver(void)
+{
+    //pc.printf("Enable RX\n");
+    //RFCommand(0x8288);
+    _write(0x8288);
+   // rx_led = 1;
+    enableFifoFill();
+}
+
+// Disable RF Receiver
+void Alpha433::disableReceiver(void)
+{    
+   //pc.printf("Disable RX\n");
+    //RFCommand(0x8208);
+    _write(0x8208);
+   // rx_led = 0;
+    disableFifoFill();
+}
+
+// SSI FiFo Clear
+void Alpha433::clearBuffer(void)
+{
+    while(_read(0xB000) != 0); 
+}
+
+// Reset RF
+void Alpha433::rf_reset(void)
+{
+    // Chip must be deselected
+    _fss = 1;
+
+    // Setup the spi for 16 bit data, high steady state clock, second edge capture, with a 1MHz clock rate
+    _spi.format(16,0);  //Was 16,3
+    _spi.frequency(2000000);
+    _nirq.mode(PullUp);
+    _nirq.fall(this,&Alpha433::interrupt);
+    // Select the device by seting chip select low
+    _fss = 0;
+    //pc.printf("End reset\n");
+
+}
+
+void Alpha433::timeout(void)
+{
+    pc.printf("Error on read; resetting chip\n");
+    rf_init();
+}
+
+// Initialise RF
+void Alpha433::rf_init(void)
+{
+    
+    pc.printf("Init start\n");
+
+    rf_reset(); // RF Hardware Reset
+    _write(0x0000);    // read status to cancel prior interrupt           
+    pc.printf("Start setup\n");
+
+    _write(0x8000 | ALPHA433_FREQUENCY | ALPHA433_CRYSTAL_LOAD | ALPHA433_USE_FIFO);
+    _write(0x9000 | ALPHA433_PIN20 | ALPHA433_VDI_RESPONSE | ALPHA433_BANDWIDTH | ALPHA433_LNA_GAIN | ALPHA433_RSSI);
+    _write(0xC228 | ALPHA433_CLOCK_RECOVERY | ALPHA433_FILTER | ALPHA433_DQD);
+    _write(0xCA00 | ALPHA433_FIFO_LEVEL | ALPHA433_FIFO_FILL | ALPHA433_HI_SENS_RESET);
+    _write(0xC400 | ALPHA433_AFC_MODE | ALPHA433_AFC_RANGE | ALPHA433_AFC_FINE_MODE | ALPHA433_AFC);
+    _write(0x9800 | ALPHA433_MOD_POLARITY | ALPHA433_MOD_FREQUENCY | ALPHA433_TX_POWER);
+    _write(0xC000 | ALPHA433_CLK_OUT | ALPHA433_LOW_BAT);
+
+    enableReceiver();
+    //pc.printf("End setup\n");
+
+    ssReceivePointer = 0;
+    reset_timeout.attach(this,&Alpha433::timeout,TIMEOUT);
+    pc.printf("Init end\n");
+    cRFStatus = ALPHA433_MODE_RECEIVING;
+   
+}
+    
+  
+// RF Interrupt
+void Alpha433::interrupt(void)
+{
+   if(cRFStatus == ALPHA433_MODE_RECEIVING){
+      irqled=1;
+      //Add reset timeout
+      reset_timeout.detach(); 
+      reset_timeout.attach(this,&Alpha433::timeout,0.5);
+      pc.printf("Rec. ISR\n");
+      int res = _read(0x0000);
+      if(res==0) res = _read(0x0000);
+      char read_failure = 0;
+
+      if (res & (ALPHA433_STATUS_TX_NEXT_BYTE | ALPHA433_STATUS_FIFO_LIMIT_REACHED)) { // RF: waiting for next Byte OR FIFO full
+        pc.printf("Receiving");
+        cRXBuffer[ssReceivePointer] = _read(0xB000) & 0xFF; // get data
+        if(ssReceivePointer == 0) {
+           ssReceiveCount = cRXBuffer[0];
+       
+           if((ssReceiveCount == 0) || (ssReceiveCount > 62)) { // error amount of data
+             read_failure=1;
+             pc.printf("Error amount of RX data: %d\n",ssReceiveCount);
+             reset_timeout.detach();
+             reset_timeout.attach(this,&Alpha433::timeout,TIMEOUT);
+           } else ssReceiveCount += 2;   // add count + checksum
+        }
+        if(!read_failure){
+          ssReceivePointer++;
+          if (ssReceivePointer > ssReceiveCount) { // End transmission
+             disableFifoFill();
+             enableFifoFill();
+             irqled=0;      
+             reset_timeout.detach(); 
+             reset_timeout.attach(this,&Alpha433::timeout,TIMEOUT);
+             ssReceivePointer = 0;
+             dataAvailable(cRXBuffer[0], &cRXBuffer[1]);
+          }
+        }else{
+             disableFifoFill();
+             enableFifoFill();
+             ssReceivePointer = 0;
+             reset_timeout.detach();
+             reset_timeout.attach(this,&Alpha433::timeout,TIMEOUT);
+        }
+     }
+   }
+}
+
+// RF Set Datarate
+void Alpha433::setDatarate(unsigned long ulValue)
+{
+    unsigned long ulRateCmd;
+    if(ulValue < 3000) ulRateCmd = 0x0080 | (10000000 / 29 / 8 / ulValue) - 1;
+    else ulRateCmd = 0x0000 | (10000000 / 29 / 1 / ulValue) - 1;
+    _write(0xC600 | ulRateCmd);
+}
+
+// RF Set Frequency
+void Alpha433::setFrequency(unsigned long ulValue)
+{
+    unsigned long ulRateCmd;
+
+#if (ALPHA433_FREQUENCY  == ALPHA433_FREQUENCY_315)
+    ulRateCmd = (ulValue - 10000000 * 1 * 31) * 4 / 10000;
+
+#elif (ALPHA433_FREQUENCY == ALPHA433_FREQUENCY_433)
+    ulRateCmd = (ulValue - 10000000 * 1 * 43) * 4 / 10000;
+
+#elif (ALPHA433_FREQUENCY == ALPHA433_FREQUENCY_868)
+    ulRateCmd = (ulValue - 10000000 * 2 * 43) * 4 / 10000;
+
+#elif (ALPHA433_FREQUENCY == ALPHA433_FREQUENCY_915)
+    ulRateCmd = (ulValue - 10000000 * 3 * 30) * 4 / 10000;
+#endif
+
+    _write(0xA000 | ulRateCmd);
+}
+
+
+
+// Enable RF Receiver FiFo fill
+void Alpha433::enableFifoFill(void)
+{
+    _write(0xCA00 | ALPHA433_FIFO_LEVEL | ALPHA433_FIFO_FILL | ALPHA433_HI_SENS_RESET | 0x0002);
+    while((_read(0x0000) & ALPHA433_STATUS_FIFO_EMPTY) == 0);
+}
+
+// Disable RF Receiver FiFo fill
+void Alpha433::disableFifoFill(void)
+{
+    _write(0xCA00 | ALPHA433_FIFO_LEVEL | ALPHA433_FIFO_FILL | ALPHA433_HI_SENS_RESET);
+}
+
+// Handle new RF Data
+void Alpha433::dataAvailable(char cCount, char* cBuffer)
+{
+   char rstring [cCount+1];
+   char checksum = 0;
+   int i;
+   for(i=0;i<cCount;i++){
+    rstring[i]=cBuffer[i];
+    checksum ^= rstring[i];
+   }
+   rstring[cCount]=0;
+   if (cBuffer[cCount] != checksum){
+        pc.printf("Received [%d] \"%s\" (checksum failed: expected %02X, received %02X)%02X %02X\n",cCount,rstring,checksum,cBuffer[cCount],cBuffer[cCount-1],cBuffer[cCount+1]);
+   }else {
+   pc.printf("Received [%d] \"%s\" (checksum passed)\n",cCount,rstring);
+   handleData(rstring, cCount);
+   }
+}
+
+
+int Alpha433::readStatusByte()
+{
+   pc.printf("Reading status byte\n");
+   return _read(0x0000); 
+}
+
+//-----PRIVATE FUNCTIONS-----
+
+void Alpha433::_write(int address) {
+    _fss=0;                 //select the deivce
+    _spi.write(address);    //write the address of where the data is to be written first
+    //pc.printf("Write data: %04X\n",address);
+    _fss=1;                 //deselect the device
+}
+
+int Alpha433::_read(int address) {
+    int _data;
+    _fss=0;                  //select the device
+    _data = _spi.write(address);     //select the register
+    //pc.printf("Read data: %04X\n",_data);
+    _fss=1;                  //deselect the device
+    return _data;            //return the data
+
+}
+
+int Alpha433::_putc (int c) {
+    return(c);
+}
+
+int Alpha433::_getc (void) {
+    char r = 0;
+    return(r);
+}
\ No newline at end of file
diff -r 000000000000 -r d63a63feb104 alpha433.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/alpha433.h	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,304 @@
+/* University of York Robot Lab m3pi Library: 433MHz Alpha Transceiver
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * October 2013
+ *
+ * Designed for use with the enhanced MBED sensor board
+ *
+ * Based on code developed by Tobias Dipper, University of Stuttgart
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef ALPHA433_H
+#define ALPHA433_H
+
+
+//
+// Defines
+//
+#define ALPHA433_FREQUENCY_315     0x0000
+#define ALPHA433_FREQUENCY_433     0x0010
+#define ALPHA433_FREQUENCY_868     0x0020
+#define ALPHA433_FREQUENCY_915     0x0030
+#define ALPHA433_CRYSTAL_LOAD_85   0x0000
+#define ALPHA433_CRYSTAL_LOAD_90   0x0001
+#define ALPHA433_CRYSTAL_LOAD_95   0x0002
+#define ALPHA433_CRYSTAL_LOAD_100  0x0003
+#define ALPHA433_CRYSTAL_LOAD_105  0x0004
+#define ALPHA433_CRYSTAL_LOAD_110  0x0005
+#define ALPHA433_CRYSTAL_LOAD_115  0x0006
+#define ALPHA433_CRYSTAL_LOAD_120  0x0007
+#define ALPHA433_CRYSTAL_LOAD_125  0x0008
+#define ALPHA433_CRYSTAL_LOAD_130  0x0009
+#define ALPHA433_CRYSTAL_LOAD_135  0x000A
+#define ALPHA433_CRYSTAL_LOAD_140  0x000B
+#define ALPHA433_CRYSTAL_LOAD_145  0x000C
+#define ALPHA433_CRYSTAL_LOAD_150  0x000D
+#define ALPHA433_CRYSTAL_LOAD_155  0x000E
+#define ALPHA433_CRYSTAL_LOAD_160  0x000F
+#define ALPHA433_USE_FIFO_YES      0x00C0
+#define ALPHA433_USE_FIFO_NO       0x0000
+#define ALPHA433_PIN20_INTERRUPT_IN    0x0000
+#define ALPHA433_PIN20_VDI_OUT         0x0400
+#define ALPHA433_VDI_RESPONSE_FAST     0x0000
+#define ALPHA433_VDI_RESPONSE_MEDIUM   0x0100
+#define ALPHA433_VDI_RESPONSE_SLOW     0x0200
+#define ALPHA433_VDI_RESPONSE_ALWAYS   0x0300
+#define ALPHA433_BANDWIDTH_400         0x0020
+#define ALPHA433_BANDWIDTH_340         0x0040
+#define ALPHA433_BANDWIDTH_270         0x0060
+#define ALPHA433_BANDWIDTH_200         0x0080
+#define ALPHA433_BANDWIDTH_134         0x00A0
+#define ALPHA433_BANDWIDTH_67          0x00C0
+#define ALPHA433_LNA_GAIN_0            0x0000
+#define ALPHA433_LNA_GAIN_6            0x0080
+#define ALPHA433_LNA_GAIN_14           0x0100
+#define ALPHA433_LNA_GAIN_20           0x0180
+#define ALPHA433_RSSI_103              0x0000
+#define ALPHA433_RSSI_97               0x0001
+#define ALPHA433_RSSI_91               0x0002
+#define ALPHA433_RSSI_85               0x0003
+#define ALPHA433_RSSI_79               0x0004
+#define ALPHA433_RSSI_73               0x0005
+#define ALPHA433_RSSI_67               0x0006
+#define ALPHA433_RSSI_61               0x0007
+#define ALPHA433_CLOCK_RECOVERY_AUTO   0x0080
+#define ALPHA433_CLOCK_RECOVERY_FAST   0x0040
+#define ALPHA433_CLOCK_RECOVERY_SLOW   0x0000
+#define ALPHA433_FILTER_DIGITAL        0x0000
+#define ALPHA433_FILTER_ANALOG         0x0010
+#define ALPHA433_DQD_0                 0x0000
+#define ALPHA433_DQD_1                 0x0001
+#define ALPHA433_DQD_2                 0x0002
+#define ALPHA433_DQD_3                 0x0003
+#define ALPHA433_DQD_4                 0x0004
+#define ALPHA433_DQD_5                 0x0005
+#define ALPHA433_DQD_6                 0x0006
+#define ALPHA433_DQD_7                 0x0007
+#define ALPHA433_FIFO_LEVEL_0          0x0000
+#define ALPHA433_FIFO_LEVEL_1          0x0010
+#define ALPHA433_FIFO_LEVEL_2          0x0020
+#define ALPHA433_FIFO_LEVEL_3          0x0030
+#define ALPHA433_FIFO_LEVEL_4          0x0040
+#define ALPHA433_FIFO_LEVEL_5          0x0050
+#define ALPHA433_FIFO_LEVEL_6          0x0060
+#define ALPHA433_FIFO_LEVEL_7          0x0070
+#define ALPHA433_FIFO_LEVEL_8          0x0080
+#define ALPHA433_FIFO_LEVEL_9          0x0090
+#define ALPHA433_FIFO_LEVEL_10         0x00A0
+#define ALPHA433_FIFO_LEVEL_11         0x00B0
+#define ALPHA433_FIFO_LEVEL_12         0x00C0
+#define ALPHA433_FIFO_LEVEL_13         0x00D0
+#define ALPHA433_FIFO_LEVEL_14         0x00E0
+#define ALPHA433_FIFO_LEVEL_15         0x00F0
+#define ALPHA433_FIFO_FILL_PATTERN     0x0000
+#define ALPHA433_FIFO_FILL_ALWAYS      0x0004
+#define ALPHA433_HI_SENS_RESET_ENABLE  0x0000
+#define ALPHA433_HI_SENS_RESET_DISABLE 0x0001
+#define ALPHA433_AFC_MODE_NOAUTO       0x0000
+#define ALPHA433_AFC_MODE_ONCE         0x0040
+#define ALPHA433_AFC_MODE_VDI          0x0080
+#define ALPHA433_AFC_MODE_INDEPENDENT  0x00C0
+#define ALPHA433_AFC_RANGE_3TO4        0x0030
+#define ALPHA433_AFC_RANGE_7TO8        0x0020
+#define ALPHA433_AFC_RANGE_15TO16      0x0010
+#define ALPHA433_AFC_RANGE_NO_RES      0x0000
+#define ALPHA433_AFC_FINE_ENABLE       0x0004
+#define ALPHA433_AFC_FINE_DISABLE      0x0000
+#define ALPHA433_AFC_ENABLE            0x0003
+#define ALPHA433_AFC_DISABLE           0x0000
+#define ALPHA433_MOD_POLARITY_P        0x0000
+#define ALPHA433_MOD_POLARITY_N        0x0100
+#define ALPHA433_MOD_FREQUENCY_15       0x0000
+#define ALPHA433_MOD_FREQUENCY_30       0x0010
+#define ALPHA433_MOD_FREQUENCY_45       0x0020
+#define ALPHA433_MOD_FREQUENCY_60       0x0030
+#define ALPHA433_MOD_FREQUENCY_75       0x0040
+#define ALPHA433_MOD_FREQUENCY_90       0x0050
+#define ALPHA433_MOD_FREQUENCY_105      0x0060
+#define ALPHA433_MOD_FREQUENCY_120      0x0070
+#define ALPHA433_MOD_FREQUENCY_135      0x0080
+#define ALPHA433_MOD_FREQUENCY_150      0x0090
+#define ALPHA433_MOD_FREQUENCY_165      0x00A0
+#define ALPHA433_MOD_FREQUENCY_180      0x00B0
+#define ALPHA433_MOD_FREQUENCY_195      0x00C0
+#define ALPHA433_MOD_FREQUENCY_210      0x00D0
+#define ALPHA433_MOD_FREQUENCY_225      0x00E0
+#define ALPHA433_MOD_FREQUENCY_240      0x00F0
+#define ALPHA433_TX_POWER_0            0x0000
+#define ALPHA433_TX_POWER_3            0x0001
+#define ALPHA433_TX_POWER_6            0x0002
+#define ALPHA433_TX_POWER_9            0x0003
+#define ALPHA433_TX_POWER_12           0x0004
+#define ALPHA433_TX_POWER_15           0x0005
+#define ALPHA433_TX_POWER_18           0x0006
+#define ALPHA433_TX_POWER_21           0x0007
+#define ALPHA433_CLK_OUT_1             0x0000
+#define ALPHA433_CLK_OUT_125           0x0020
+#define ALPHA433_CLK_OUT_166           0x0040
+#define ALPHA433_CLK_OUT_2             0x0060
+#define ALPHA433_CLK_OUT_25            0x0080
+#define ALPHA433_CLK_OUT_333           0x00A0
+#define ALPHA433_CLK_OUT_5             0x00C0
+#define ALPHA433_CLK_OUT_10            0x00E0
+#define ALPHA433_LOW_BAT22             0x0000
+#define ALPHA433_STATUS_TX_NEXT_BYTE           0x8000
+#define ALPHA433_STATUS_FIFO_LIMIT_REACHED     0x8000
+#define ALPHA433_STATUS_POWER_ON_RESET         0x4000
+#define ALPHA433_STATUS_RX_OVERFLOW            0x2000
+#define ALPHA433_STATUS_TX_UNDERRUN            0x2000
+#define ALPHA433_STATUS_WAKEUP                 0x1000
+#define ALPHA433_STATUS_EXT                    0x0800
+#define ALPHA433_STATUS_LOW_BATTERY            0x0400
+#define ALPHA433_STATUS_FIFO_EMPTY             0x0200
+#define ALPHA433_STATUS_STRONG_SIGNAL          0x0100
+#define ALPHA433_STATUS_RSSI                   0x0100
+#define ALPHA433_STATUS_DQD                    0x0080
+#define ALPHA433_STATUS_CLOCK_LOCKED           0x0040
+#define ALPHA433_TRANSMIT_OK                   0
+#define ALPHA433_TRANSMIT_TIMEOUT              1
+#define ALPHA433_TRANSMIT_PARITY_ERROR         2
+#define ALPHA433_RECEIVE_OK                    0
+#define ALPHA433_RECEIVE_TIMEOUT               1
+#define ALPHA433_RECEIVE_PARITY_ERROR          2
+#define ALPHA433_NODATA            0
+#define ALPHA433_DATA_AVAIABLE     1
+#define ALPHA433_MODE_TRANSMITTING         1
+#define ALPHA433_MODE_RECEIVING            2
+#define ALPHA433_MODE_SWITCHING            0
+
+// ----------------------------- default user configuration --------------------------------
+
+#define ALPHA433_FREQUENCY         ALPHA433_FREQUENCY_433
+#define ALPHA433_CRYSTAL_LOAD      ALPHA433_CRYSTAL_LOAD_100
+#define ALPHA433_USE_FIFO          ALPHA433_USE_FIFO_YES
+#define ALPHA433_PIN20             ALPHA433_PIN20_INTERRUPT_IN
+#define ALPHA433_VDI_RESPONSE      ALPHA433_VDI_RESPONSE_SLOW
+#define ALPHA433_BANDWIDTH         ALPHA433_BANDWIDTH_134
+#define ALPHA433_LNA_GAIN          ALPHA433_LNA_GAIN_0
+#define ALPHA433_RSSI              ALPHA433_RSSI_97
+#define ALPHA433_CLOCK_RECOVERY    ALPHA433_CLOCK_RECOVERY_SLOW
+#define ALPHA433_FILTER            ALPHA433_FILTER_DIGITAL
+#define ALPHA433_DQD               ALPHA433_DQD_4
+#define ALPHA433_FIFO_LEVEL        ALPHA433_FIFO_LEVEL_8
+#define ALPHA433_FIFO_FILL         ALPHA433_FIFO_FILL_PATTERN
+#define ALPHA433_HI_SENS_RESET     ALPHA433_HI_SENS_RESET_DISABLE
+#define ALPHA433_AFC_MODE          ALPHA433_AFC_MODE_INDEPENDENT
+#define ALPHA433_AFC_RANGE         ALPHA433_AFC_RANGE_3TO4
+#define ALPHA433_AFC_FINE_MODE     ALPHA433_AFC_FINE_DISABLE
+#define ALPHA433_AFC               ALPHA433_AFC_DISABLE
+#define ALPHA433_MOD_POLARITY      ALPHA433_MOD_POLARITY_P
+#define ALPHA433_MOD_FREQUENCY     ALPHA433_MOD_FREQUENCY_90
+#define ALPHA433_TX_POWER          ALPHA433_TX_POWER_0
+#define ALPHA433_CLK_OUT           ALPHA433_CLK_OUT_2
+#define ALPHA433_LOW_BAT           ALPHA433_LOW_BAT22
+#define ALPHA433_TIMEOUT   100
+#define TIMEOUT 5.0
+
+class Alpha433 : public Stream {
+
+// Public Functions
+
+public:
+
+   /** Create the alpha433 object connected to the default pins
+     *
+     * @param mosi pin - default is p5
+     * @param miso pin - default is p6
+     * @param sck pin - default is p7
+     * @param fss pin - default is p8
+     * @param nirq pin - default is p11
+     */
+    Alpha433();
+    
+    /** Create the alpha433 object connected to specific pins
+     *
+     */
+    Alpha433(PinName mosi, PinName miso, PinName sck, PinName fss, PinName nirq);
+
+
+// Send a string to the RF transmitter
+unsigned long sendString(char cCount, char* cBuffer);
+
+// Enable RF Transmitter
+void enableTransmitter(void);
+
+// Disable RF Transmitter
+void disableTransmitter(void);
+
+// Enable RF Receiver
+void enableReceiver(void);
+
+// Disable RF Receiver
+void disableReceiver(void);
+
+// SSI FiFo Clear
+void clearBuffer(void);
+
+// Reset RF
+void rf_reset(void);
+
+// Initialise RF
+void rf_init(void);
+
+// RF Interrupt
+void interrupt(void);
+
+// RF Set Datarate
+void setDatarate(unsigned long ulValue);
+
+// RF Set Frequency
+void setFrequency(unsigned long ulValue);
+
+// Enable RF Receiver FiFo fill
+void enableFifoFill(void);
+
+// Disable RF Receiver FiFo fill
+void disableFifoFill(void);
+
+// Handle new RF Data
+void dataAvailable(char cCount, char* cBuffer);
+
+// Read status byte
+int readStatusByte();
+
+// Reset timeout: stops hanging on bad receive; resets alpha 433
+void timeout();
+
+private :
+
+    DigitalOut _fss;
+    SPI _spi;
+    DigitalIn _nirq_test;
+    InterruptIn _nirq;
+        
+    //Write a byte (data) to address    
+    void _write(int address);
+
+    //Read a byte (return val) from address
+    int _read(int address);
+    
+    virtual int _putc(int c);
+    virtual int _getc();
+    
+};
+
+#endif // ALPHA433_H
\ No newline at end of file
diff -r 000000000000 -r d63a63feb104 communications.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/communications.cpp	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,1609 @@
+/*******************************************************************************************
+ *
+ * University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * Version 0.5 February 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
+ *
+ ******************************************************************************************/
+
+// Important note:  The communication stack is enabled by setting the "USE_COMMUNICATION_STACK" flag to 1
+// When being used, all received messages are decoded using the decodeMessage() function
+// See manual for more info on using the communication handler
+#include "mbed.h"
+#include "communications.h"
+#include "main.h"
+
+struct swarm_member swarm[SWARM_SIZE];      // Array to hold received information about other swarm members
+DigitalOut actioning (LED1);
+DigitalOut errorled (LED2);
+DigitalOut tx (LED3);
+DigitalOut rx (LED4);
+Timeout tdma_timeout;
+char tdma_busy = 0;
+char waiting_message [64];
+char waiting_length = 0;
+int message_id = 0;
+
+
+// Send a structured message over the RF interface
+// @target - The target recipient (1-31 or 0 for broadcast)
+// @command - The command byte (see manual)
+// @*data - Additional data bytes
+// @length - Length of additional data
+void send_rf_message(char target, char command, char * data, char length)
+{
+    char message [5+length];
+    message_id++;
+    message[0] = 32;
+    message[1] = target+32;
+    message[2] = message_id%256;
+    message[3] = command;
+    for(int i=0;i<length;i++){
+        message[4+i] = data[i];
+    }
+    message[4+length]=NULL;
+   rf.sendString(4+length,message);
+    if(RF_DEBUG==1)pc.printf("RF message sent");
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Internal Functions
+// In general these functions should not be called by user code
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Decode the received message header.  Check it is min. 4 bytes long, that the sender and target are valid [called in alpha433.cpp] 
+void processRadioData(char * data, char length)
+{
+    if(RF_USE_LEDS==1) {
+        errorled=0;
+        rx=1;
+    }
+    // Decompose the received message
+    if(length < 4) errormessage(0);
+    else {
+        // Establish the sender and target of the packet
+        char sender = data[0];
+        char target = data[1];
+        char id = data[2];
+        char command = data[3];
+        if(sender<32 || sender>63)errormessage(1);
+        else {
+            if(target<32 || target>63)errormessage(2);
+            else {
+                sender -= 32;
+                target -= 32;
+                decodeMessage(sender,target,id,command,data+4,length-4);
+            }
+        }
+    }
+    if(RF_USE_LEDS==1) rx=0;
+}
+
+//Decode the received message, action it if it is valid and for me 
+void decodeMessage(char sender, char target, char id, char command, char * data, char length)
+{
+    char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
+
+    if(target==0) broadcast_message = 1;
+    is_response = 0 != (command & (1 << 7));
+    request_response = 0 != (command & (1 << 6));
+    is_user = 0 != (command & (1 << 5));
+    is_command = 0 != (command & (1 << 4));
+    function = command % 16;
+
+    if (RF_DEBUG==1) {
+        if(is_command == 1) pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, commands_array[function],length);
+        else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length);
+    }
+
+    //Action the message only if I am a recipient
+    if(target==0) {
+        if(RF_USE_LEDS==1) actioning = 1;
+        if(is_response == 1) {
+            if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
+            else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
+        } else {
+            if(is_command == 1) {
+                if(RF_ALLOW_COMMANDS == 1) {
+                    if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
+                    else handle_command(sender, broadcast_message, request_response, id, function, data, length);
+                } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
+            } else {
+                //A information request has no extra parameters
+                if(length == 0) {
+                    if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
+                    else handle_request(sender, broadcast_message, request_response, id, function);
+                } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
+            }
+        }
+        if(RF_USE_LEDS==1) actioning = 0;
+    } else  if (RF_DEBUG==1) pc.printf(" - Ignored\n");
+}
+
+//Send a predefined response message
+void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
+{
+    char message [4+length];
+    message[0]=0;
+    message[1]=target;
+    message[2]=id;
+    message[3]=128 + (success << 6) + (is_command << 4) + function;
+    for(int i=0; i<length; i++) {
+        message[4+i]=data[i];
+    }
+    /*TDMA not relevant to handheld controller
+    //Delay the response if it is broadcast and TDMA mode is on
+    if(RF_USE_TDMA == 1 && is_broadcast == 1) {
+        if(tdma_busy == 1) {
+            if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
+        } else {
+            tdma_busy = 1;
+            strcpy(waiting_message,message);
+            waiting_length=length;
+            tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
+            if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
+        }
+    } else {
+    */
+        rf.sendString(4+length,message);
+        if(RF_DEBUG==1)pc.printf("Response issued");
+    //}
+}
+
+// Send a delayed response
+void tdma_response()
+{
+    rf.sendString(4+waiting_length,waiting_message);
+    tdma_busy = 0;
+    if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
+}
+
+// Handle a message that is a predefined command
+void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length)
+{
+    char success = 0;
+    switch(function) {
+        case 0: // Stop             [0 data]
+            if(length==0) {
+                //piswarm.stop();
+                if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - ");
+                success = 1;
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 1: // Forward          [2 bytes: 16-bit signed short]
+            if(length==2) {
+                int  i_speed = (data[0] << 8) + data[1];
+                float speed = i_speed / 32768.0;
+                speed--;
+                //piswarm.forward(speed);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 2: // Backward         [2 bytes: 16-bit signed short]
+            if(length==2) {
+                int  i_speed = (data[0] << 8) + data[1];
+                float speed = i_speed / 32768.0;
+                speed--;
+                //piswarm.backward(speed);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 3: // Left             [2 bytes: 16-bit signed short]
+            if(length==2) {
+                int  i_speed = (data[0] << 8) + data[1];
+                float speed = i_speed / 32768.0;
+                speed--;
+                //piswarm.left(speed);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 4: // Right            [2 bytes: 16-bit signed short]
+            if(length==2) {
+                int  i_speed = (data[0] << 8) + data[1];
+                float speed = i_speed / 32768.0;
+                speed--;
+                //piswarm.right(speed);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 5: // Left Motor       [2 bytes: 16-bit signed short]
+            if(length==2) {
+                int  i_speed = (data[0] << 8) + data[1];
+                float speed = i_speed / 32768.0;
+                speed--;
+                //piswarm.left_motor(speed);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 6: // Right Motor      [2 bytes: 16-bit signed short]
+            if(length==2) {
+                int  i_speed = (data[0] << 8) + data[1];
+                float speed = i_speed / 32768.0;
+                speed--;
+                //piswarm.right_motor(speed);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 7: // Outer LED Colour [3 bytes: R, G, B]
+            if(length==3) {
+                //piswarm.set_oled_colour (data[0],data[1],data[2]);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 8: // Center LED Colour[3 bytes: R, G, B]
+            if(length==3) {
+                //piswarm.set_cled_colour (data[0],data[1],data[2]);
+                success = 1;
+                if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 9: // Outer LED State  [2 bytes: [xxxxxx01][23456789] ]
+            if(length==2) {
+                //piswarm.set_oleds(0 != (data[0] & (1 << 1)),0 != (data[0] & (1 << 0)),0 != (data[1] & (1 << 7)),0 != (data[1] & (1 << 6)),0 != (data[1] & (1 << 5)),0 != (data[1] & (1 << 4)),0 != (data[1] & (1 << 3)),0 != (data[1] & (1 << 2)),0 != (data[1] & (1 << 1)),0 != (data[1] & (1 << 0)));
+                success = 1;
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
+            if(length==1) {
+                //piswarm.enable_cled (data[0] % 2);
+                success = 1;
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 11: // Set outer LED    [1 byte:  [xxxEvvvv] E=enabled vvvv=LED]
+            if(length==1) {
+                int led = data[0] % 16;
+                if(led < 10) {
+                  //  piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
+                    success = 1;
+                } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 12: // Play sound      [Minimum 1 byte]
+            if(length>0) {
+                //piswarm.play_tune(data,length);
+                success = 1;
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 13: // Sync time
+            if(length==4) {
+                unsigned int new_time = 0;
+                new_time+=((unsigned int)data[0] << 24);
+                new_time+=((unsigned int)data[1] << 16);
+                new_time+=((unsigned int)data[2] << 8);
+                new_time+=(unsigned int)data[3];
+                set_time(new_time);
+                //display_system_time();
+            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+            break;
+        case 14: //
+            break;
+        case 15: //
+            break;
+    }
+    if(request_response == 1) {
+        send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
+    }
+
+}
+
+//Handle a message that is a predefined request
+void handle_request(char sender, char is_broadcast, char request_response, char id, char function)
+{
+    int response_length = 0;
+    char * response = NULL;
+    char success = 0;
+
+    switch(function) {
+        case 0: // Null request
+            success=1;
+            break;
+        case 1: { // Request left motor speed
+            response_length = 2;
+            float speed = 0;//piswarm.get_left_motor() * 32767;
+            int a_speed = 32768 + (int) speed;
+            char msb = (char) (a_speed / 256);
+            char lsb = (char) (a_speed % 256);
+            response = new char[2];
+            response[0]=msb;
+            response[1]=lsb;
+            success=1;
+            break;
+        }
+        case 2: { // Request right motor speed
+            response_length = 2;
+            float speed = 0;//piswarm.get_right_motor() * 32767;
+            int a_speed = 32768 + (int) speed;
+            char msb = (char) (a_speed / 256);
+            char lsb = (char) (a_speed % 256);
+            response = new char[2];
+            response[0]=msb;
+            response[1]=lsb;
+            success=1;
+            break;
+        }
+        case 3: { // Request button state
+            response_length = 1;
+            response = new char[1];
+            response[0]=0;//piswarm.get_switches();
+            break;
+        }
+        case 4: { // Request LED colours
+            response_length = 6;
+            response = new char[6];
+            int oled_colour = 0;//piswarm.get_oled_colour();
+            int cled_colour = 0;//piswarm.get_cled_colour();
+            response[0] = (char) (oled_colour >> 16);
+            response[1] = (char) ((oled_colour >> 8) % 256);
+            response[2] = (char) (oled_colour % 256);
+            response[3] = (char) (cled_colour >> 16);
+            response[4] = (char) ((cled_colour >> 8) % 256);
+            response[5] = (char) (cled_colour % 256);
+            break;
+        }
+        case 5: { // Request LED states
+            response_length = 2;
+            response = new char[2];
+            response[0] = 0;
+            response[1] = 0;
+            //if (piswarm.get_cled_state() == 1) response[0] += 4;
+            //if (piswarm.get_oled_state(0) == 1) response[0] += 2;
+            //if (piswarm.get_oled_state(1) == 1) response[0] += 1;
+            //if (piswarm.get_oled_state(2) == 1) response[1] += 128;
+            //if (piswarm.get_oled_state(3) == 1) response[1] += 64;
+            //if (piswarm.get_oled_state(4) == 1) response[1] += 32;
+            //if (piswarm.get_oled_state(5) == 1) response[1] += 16;
+            //if (piswarm.get_oled_state(6) == 1) response[1] += 8;
+            //if (piswarm.get_oled_state(7) == 1) response[1] += 4;
+            //if (piswarm.get_oled_state(8) == 1) response[1] += 2;
+            //if (piswarm.get_oled_state(9) == 1) response[1] += 1;
+            break;
+        }
+        case 6: { // Request battery power
+            response_length = 2;
+            response = new char[2];
+            float fbattery = 0;//piswarm.battery() * 1000.0;
+            unsigned short battery = (unsigned short) fbattery;
+            response[0] = battery >> 8;
+            response[1] = battery % 256;
+            break;
+        }
+        case 7: { // Request light sensor reading
+            response_length = 2;
+            response = new char[2];
+            float flight = 0;//piswarm.read_light_sensor() * 655.0;
+            unsigned short light = (unsigned short) flight;
+            response[0] = light >> 8;
+            response[1] = light % 256;
+            break;
+        }
+        case 8: { // Request accelerometer reading
+            response_length = 6;
+            response = new char[6];
+            int acc_x = 0;//(int)piswarm.read_accelerometer_x();
+            int acc_y = 0;//(int)piswarm.read_accelerometer_y();
+            int acc_z = 0;//(int)piswarm.read_accelerometer_z();
+            acc_x += 32768;
+            acc_y += 32768;
+            acc_z += 32768;
+            response[0]=acc_x >> 8;
+            response[1]=acc_x % 256;
+            response[2]=acc_y >> 8;
+            response[3]=acc_y % 256;
+            response[4]=acc_z >> 8;
+            response[5]=acc_z % 256;
+            break;
+        }
+        case 9: { // Request gyroscope reading
+            response_length = 2;
+            response = new char[2];
+            float fgyro = 0;//piswarm.read_gyro();
+            fgyro += 32768;
+            unsigned short sgyro = (unsigned short) fgyro;
+            response[0] = sgyro >> 8;
+            response[1] = sgyro % 256;
+            break;
+        }
+        case 10: { // Request background IR reading
+            response_length = 16;
+            response = new char[16];
+            for(int sensor = 0; sensor < 8; sensor ++){
+                int offset = sensor * 2;
+                unsigned short m_val = 0;//piswarm.read_adc_value(sensor);
+                response[offset]=m_val >> 8;
+                response[offset+1]=m_val % 256;
+            }
+            break;
+        }
+        case 11: { // Request illuminated IR reading
+            response_length = 16;
+            response = new char[16];
+            for(int sensor = 0; sensor < 8; sensor ++){
+                int offset = sensor * 2;
+                unsigned short m_val = 0;//piswarm.read_illuminated_raw_ir_value(sensor);
+                response[offset]=m_val >> 8;
+                response[offset+1]=m_val % 256;
+            }
+            break;
+        }
+
+        case 12: { // Request distance IR reading
+            response_length = 16;
+            response = new char[16];
+            for(int sensor = 0; sensor < 8; sensor ++){
+                int offset = sensor * 2;
+                float f_val = 0;//piswarm.read_reflected_ir_distance(sensor);
+                //f_val ranges from 0 to 100.0;
+                f_val *= 655;
+                unsigned short m_val = (unsigned short) f_val;
+                response[offset]=m_val >> 8;
+                response[offset+1]=m_val % 256;
+            }
+            break;
+        }
+            
+        case 13: // Request line-tracking IR reading
+            break;
+        case 14: // Request uptime
+            break;
+        case 15: //
+            break;
+    }
+    send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
+
+}
+
+
+void errormessage(int index)
+{
+    if(RF_USE_LEDS==1) errorled=1;
+    switch(index) {
+        case 0: //Message to short
+            if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
+            break;
+        case 1: //Sender out of valid range
+            if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
+            break;
+        case 2: //Target out of valid range
+            if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
+            break;
+        case 3:
+
+            break;
+        case 4:
+            break;
+    }
+}
+
+
+
+
+
+void send_rf_request_null ( char target )
+{
+    char command = 0x80;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) { //Request broadcast to all recipients
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_null = 1;
+        }
+    } else swarm[target].status_rf_request_null = 1;
+    send_rf_message(target,command,data,length);
+}
+
+
+void send_rf_request_left_motor_speed ( char target )
+{
+    char command = 0x81;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_left_motor_speed = 1;
+        }
+    } else swarm[target].status_rf_request_left_motor_speed = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_right_motor_speed ( char target )
+{
+    char command = 0x82;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_right_motor_speed = 1;
+        }
+    } else swarm[target].status_rf_request_right_motor_speed = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_button_state ( char target )
+{
+    char command = 0x83;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_button_state = 1;
+        }
+    } else swarm[target].status_rf_request_button_state = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_led_colour ( char target )
+{
+    char command = 0x84;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_led_colour = 1;
+        }
+    } else swarm[target].status_rf_request_led_colour = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_led_states ( char target )
+{
+    char command = 0x85;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_led_states = 1;
+        }
+    } else swarm[target].status_rf_request_led_states = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_battery ( char target )
+{
+    char command = 0x86;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_battery = 1;
+        }
+    } else swarm[target].status_rf_request_battery = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_light_sensor ( char target )
+{
+    char command = 0x87;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_light_sensor = 1;
+        }
+    } else swarm[target].status_rf_request_light_sensor = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_accelerometer ( char target )
+{
+    char command = 0x88;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_accelerometer = 1;
+        }
+    } else swarm[target].status_rf_request_accelerometer = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_gyroscope ( char target )
+{
+    char command = 0x89;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_gyroscope = 1;
+        }
+    } else swarm[target].status_rf_request_gyroscope = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_background_ir ( char target )
+{
+    char command = 0x8A;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_background_ir = 1;
+        }
+    } else swarm[target].status_rf_request_background_ir = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_reflected_ir ( char target )
+{
+    char command = 0x8B;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_reflected_ir = 1;
+        }
+    } else swarm[target].status_rf_request_reflected_ir = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_distance_ir ( char target )
+{
+    char command = 0x8C;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_distance_ir = 1;
+        }
+    } else swarm[target].status_rf_request_distance_ir = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_line_following_ir ( char target )
+{
+    char command = 0x8D;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_line_following_ir = 1;
+        }
+    } else swarm[target].status_rf_request_line_following_ir = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_uptime ( char target )
+{
+    char command = 0x8E;
+    char length = 0;
+    char * data = NULL;
+    if(target == 0) {
+        for(int i=0; i<SWARM_SIZE; i++) {
+            swarm[i].status_rf_request_uptime= 1;
+        }
+    } else swarm[target].status_rf_request_uptime = 1;
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_stop ( char target, char request_response )
+{
+    char command = 0x10;
+    char length = 0;
+    char data [0];
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_stop= 1;
+            }
+        } else swarm[target].status_rf_command_stop = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_forward ( char target, char request_response, float speed )
+{
+    char command = 0x11;
+    char length = 2;
+    char data [2];
+    float qspeed = speed + 1;
+    qspeed *= 32768.0;
+    int ispeed = (int) qspeed;
+    data[0] = (char) (ispeed >> 8);
+    data[1] = (char) (ispeed % 256);
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_forward= 1;
+            }
+        } else swarm[target].status_rf_command_forward = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+
+void send_rf_command_backward ( char target, char request_response, float speed )
+{
+    char command = 0x12;
+    char length = 2;
+    char data [2];
+    float qspeed = speed + 1;
+    qspeed *= 32768.0;
+    int ispeed = (int) qspeed;
+    data[0] = (char) (ispeed >> 8);
+    data[1] = (char) (ispeed % 256);
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_backward= 1;
+            }
+        } else swarm[target].status_rf_command_backward = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_left ( char target, char request_response, float speed )
+{
+    char command = 0x13;
+    char length = 2;
+    char data [2];
+    float qspeed = speed + 1;
+    qspeed *= 32768.0;
+    int ispeed = (int) qspeed;
+    data[0] = (char) (ispeed >> 8);
+    data[1] = (char) (ispeed % 256);
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_left = 1;
+            }
+        } else swarm[target].status_rf_command_left = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_right ( char target, char request_response, float speed )
+{
+    char command = 0x14;
+    char length = 2;
+    char data [2];
+    float qspeed = speed + 1;
+    qspeed *= 32768.0;
+    int ispeed = (int) qspeed;
+    data[0] = (char) (ispeed >> 8);
+    data[1] = (char) (ispeed % 256);
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_right = 1;
+            }
+        } else swarm[target].status_rf_command_right = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_left_motor ( char target, char request_response, float speed )
+{
+    char command = 0x15;
+    char length = 2;
+    char data [2];
+    float qspeed = speed + 1;
+    qspeed *= 32768.0;
+    int ispeed = (int) qspeed;
+    data[0] = (char) (ispeed >> 8);
+    data[1] = (char) (ispeed % 256);
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_left_motor = 1;
+            }
+        } else swarm[target].status_rf_command_left_motor = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_right_motor ( char target, char request_response, float speed )
+{
+    char command = 0x16;
+    char length = 2;
+    char data [2];
+    float qspeed = speed + 1;
+    qspeed *= 32768.0;
+    int ispeed = (int) qspeed;
+    data[0] = (char) (ispeed >> 8);
+    data[1] = (char) (ispeed % 256);
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_right_motor = 1;
+            }
+        } else swarm[target].status_rf_command_right_motor = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue )
+{
+    char command = 0x17;
+    char length = 3;
+    char data [3];
+    data[0] = red;
+    data[1] = green;
+    data[2] = blue;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_oled_colour = 1;
+            }
+        } else swarm[target].status_rf_command_oled_colour = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue )
+{
+    char command = 0x18;
+    char length = 3;
+    char data [3];
+    data[0] = red;
+    data[1] = green;
+    data[2] = blue;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_cled_colour = 1;
+            }
+        } else swarm[target].status_rf_command_cled_colour = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_oled_state ( char target, char request_response, char led0, char led1, char led2, char led3, char led4, char led5, char led6, char led7, char led8, char led9 )
+{
+    char command = 0x19;
+    char length = 2;
+    char data [2];
+    data[0] = 0;
+    data[1] = 0;
+    if( led0 == 1) data[0] += 2;
+    if( led1 == 1) data[0] += 1;
+    if( led2 == 1) data[1] += 128;
+    if( led3 == 1) data[1] += 64;
+    if( led4 == 1) data[1] += 32;
+    if( led5 == 1) data[1] += 16;
+    if( led6 == 1) data[1] += 8;
+    if( led7 == 1) data[1] += 4;
+    if( led8 == 1) data[1] += 2;
+    if( led9 == 1) data[1] += 1;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_oled_state = 1;
+            }
+        } else swarm[target].status_rf_command_oled_state = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_cled_state ( char target, char request_response, char enable )
+{
+    char command = 0x1A;
+    char length = 1;
+    char data [1];
+    data[0] = enable;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_cled_state = 1;
+            }
+        } else swarm[target].status_rf_command_cled_state = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_set_oled ( char target, char request_response, char oled, char enable )
+{
+    char command = 0x1B;
+    char length = 1;
+    char data [1];
+    data[0] = oled;
+    if(enable == 1) oled+= 32;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_set_oled = 1;
+            }
+        } else swarm[target].status_rf_command_set_oled = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_play_tune ( char target, char request_response, char * data, char length )
+{
+    char command = 0x1C;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_play_tune = 1;
+            }
+        } else swarm[target].status_rf_command_play_tune = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_sync_time ( char target, char request_response )
+{
+    char command = 0x1D;
+    char length = 1;
+    char data [1];
+    data[0] = 0;
+    if(request_response == 1) {
+        command+=64;
+        if(target == 0) {
+            for(int i=0; i<SWARM_SIZE; i++) {
+                swarm[i].status_rf_command_sync_time = 1;
+            }
+        } else swarm[target].status_rf_command_sync_time = 1;
+    }
+    send_rf_message(target,command,data,length);
+}
+
+//Resets the recorded swarm data tables
+void setup_communications()
+{
+    for(int i=0; i<SWARM_SIZE; i++) {
+        swarm[i].status_rf_request_null = 0;
+        swarm[i].status_rf_request_left_motor_speed = 0;
+        swarm[i].status_rf_request_right_motor_speed = 0;
+        swarm[i].status_rf_request_button_state = 0;
+        swarm[i].status_rf_request_led_colour = 0;
+        swarm[i].status_rf_request_led_states = 0;
+        swarm[i].status_rf_request_battery = 0;
+        swarm[i].status_rf_request_light_sensor = 0;
+        swarm[i].status_rf_request_accelerometer = 0;
+        swarm[i].status_rf_request_gyroscope = 0;
+        swarm[i].status_rf_request_background_ir = 0;
+        swarm[i].status_rf_request_reflected_ir = 0;
+        swarm[i].status_rf_request_distance_ir = 0;
+        swarm[i].status_rf_request_line_following_ir = 0;
+        swarm[i].status_rf_request_uptime = 0;
+        swarm[i].status_rf_command_stop = 0;
+        swarm[i].status_rf_command_forward = 0;
+        swarm[i].status_rf_command_backward = 0;
+        swarm[i].status_rf_command_left = 0;
+        swarm[i].status_rf_command_right = 0;
+        swarm[i].status_rf_command_left_motor = 0;
+        swarm[i].status_rf_command_right_motor = 0;
+        swarm[i].status_rf_command_oled_colour = 0;
+        swarm[i].status_rf_command_cled_colour = 0;
+        swarm[i].status_rf_command_oled_state = 0;
+        swarm[i].status_rf_command_cled_state = 0;
+        swarm[i].status_rf_command_set_oled = 0;
+        swarm[i].status_rf_command_play_tune = 0;
+        swarm[i].status_rf_command_sync_time = 0;
+        swarm[i].left_motor_speed = 0.0;
+        swarm[i].right_motor_speed = 0.0;
+        swarm[i].button_state = 0;
+        for(int k=0; k<3; k++) {
+            swarm[i].outer_led_colour [k]=0;
+            swarm[i].center_led_colour [k]=0;
+            swarm[i].accelerometer [k]=0;
+        }
+        swarm[i].led_states[0]=0;
+        swarm[i].led_states[1]=0;
+        swarm[i].battery = 0.0;
+        swarm[i].light_sensor = 0.0;
+        swarm[i].gyro = 0.0;
+        for(int k=0; k<8; k++) {
+          swarm[i].background_ir[k] = 0;
+          swarm[i].reflected_ir[k] = 0;
+          swarm[i].distance_ir[k] = 0;
+        }
+    }
+}
+
+
+// Handle a message that is a response to a predefined (non-user) command or request
+void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
+{
+    char outcome = 0;
+    if(is_command == 0) {
+        // Response is a data_request_response
+        switch(function) {
+            case 0: {
+                if(swarm[sender].status_rf_request_null == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_request_null = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_null= 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_null = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 1: { //Left motor speed
+                if(swarm[sender].status_rf_request_left_motor_speed == 1) {
+                    if(success == 1){
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_left_motor_speed = 2;
+                            int value = (data [0] << 8) + data[1];
+                            value -= 32768;
+                            float val = value;
+                            val /= 32767.0;
+                            swarm[sender].left_motor_speed = val;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_left_motor_speed= 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_left_motor_speed = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 2: { //Right motor speed
+                if(swarm[sender].status_rf_request_right_motor_speed == 1) {
+                    if(success == 1){
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_right_motor_speed = 2;
+                            int value = (data [0] << 8) + data[1];
+                            value -= 32768;
+                            float val = value;
+                            val /= 32767.0;
+                            swarm[sender].right_motor_speed = val;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_right_motor_speed = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_right_motor_speed = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 3: { //Button state
+                if(swarm[sender].status_rf_request_button_state == 1) {
+                    if(success == 1){
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_button_state = 2;
+                            swarm[sender].button_state = data[0];
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_button_state = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_button_state = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 4: { //LED Colour
+                if(swarm[sender].status_rf_request_led_colour == 1) {
+                    if(success == 1) {
+                        if(length == 6) {
+                            swarm[sender].status_rf_request_led_colour = 2;
+                            for(int i=0; i<3; i++) {
+                                swarm[sender].outer_led_colour[i] = data[i];
+                                swarm[sender].center_led_colour[i] = data[i+3];
+                            }
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_led_colour = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_led_colour = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 5: { //LED States
+                if(swarm[sender].status_rf_request_led_states == 1) {
+                    if(success == 1) {
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_led_states = 2;
+                            for(int i=0; i<3; i++) {
+                                swarm[sender].led_states[0] = data[0];
+                                swarm[sender].led_states[1] = data[1];
+                            }
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_led_states = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_led_states = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+
+            case 6: { //Battery
+                if(swarm[sender].status_rf_request_battery == 1) {
+                    if(success == 1) {
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_battery = 2;
+                            int fbattery = data[0] * 256;
+                            fbattery += data[1];
+                            swarm[sender].battery = (float) fbattery / 1000.0;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_battery = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_battery = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 7: { //Light sensor
+                if(swarm[sender].status_rf_request_light_sensor == 1) {
+                    if(success == 1) {
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_light_sensor = 2;
+                            int ilight = data[0] * 256;
+                            ilight += data[1];
+                            float flight = (float) (ilight) / 655.0;
+                            swarm[sender].light_sensor = flight;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_light_sensor = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_light_sensor = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 8: { //Accelerometer
+                if(swarm[sender].status_rf_request_accelerometer == 1) {
+                    if(success == 1) {
+                        if(length == 6) {
+                            swarm[sender].status_rf_request_accelerometer = 2;
+                            int acc_x = (data[0] * 256) + data[1];
+                            int acc_y = (data[2] * 256) + data[3];
+                            int acc_z = (data[4] * 256) + data[5];
+                            swarm[sender].accelerometer[0] = (float) acc_x - 32768;
+                            swarm[sender].accelerometer[1] = (float) acc_y - 32768;
+                            swarm[sender].accelerometer[2] = (float) acc_z - 32768;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_accelerometer = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_accelerometer = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 9: { //Gyroscope
+                if(swarm[sender].status_rf_request_gyroscope == 1) {
+                    if(success == 1) {
+                        if(length == 2) {
+                            swarm[sender].status_rf_request_gyroscope = 2;
+                            int gyro = (data [0] * 256) + data[1];
+                            swarm[sender].gyro = (float) gyro - 32768;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_gyroscope = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_gyroscope = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 10: { //Background IR
+                if(swarm[sender].status_rf_request_background_ir == 1) {
+                    if(success == 1) {
+                        if(length == 16) {
+                            swarm[sender].status_rf_request_background_ir = 2;
+                            for(int i=0;i<8;i++){
+                                int offset = i * 2;
+                                unsigned short value = (unsigned short) data[offset] << 8;
+                                value += data[offset+1];
+                                swarm[sender].background_ir[i]=value;   
+                            }
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_background_ir = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_background_ir = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 11: { //Reflected IR
+                if(swarm[sender].status_rf_request_reflected_ir == 1) {
+                    if(success == 1) {
+                        if(length == 16) {
+                            swarm[sender].status_rf_request_reflected_ir = 2;
+                            for(int i=0;i<8;i++){
+                                int offset = i * 2;
+                                unsigned short value = (unsigned short) data[offset] << 8;
+                                value += data[offset+1];
+                                swarm[sender].reflected_ir[i]=value;   
+                            }
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_reflected_ir = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_reflected_ir = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 12: { // Distance IR
+                if(swarm[sender].status_rf_request_distance_ir == 1) {
+                    if(success == 1) {
+                        if(length == 16) {
+                            swarm[sender].status_rf_request_distance_ir = 2;
+                            for(int i=0;i<8;i++){
+                                int offset = i * 2;
+                                unsigned short value = (unsigned short) data[offset] << 8;
+                                value += data[offset+1];
+                                float adjusted = (float) value / 655.0;
+                                swarm[sender].distance_ir[i]=adjusted;   
+                            }
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_distance_ir = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_distance_ir = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 13: { // Line following IR
+                if(swarm[sender].status_rf_request_line_following_ir == 1) {
+                    if(success == 1) {
+                        if(length == 10) {
+                            swarm[sender].status_rf_request_line_following_ir = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_line_following_ir = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_line_following_ir = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+            case 14: { // Request uptime
+                if(swarm[sender].status_rf_request_uptime == 1) {
+                    if(success == 1) {
+                        if(length == 4) {
+                            swarm[sender].status_rf_request_uptime = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_request_uptime = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_request_uptime = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+
+        }
+    } else {
+        // Response to a command
+        switch(function) {
+            case 0: {
+                if(swarm[sender].status_rf_command_stop == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_stop = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_stop = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_stop = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 1: {
+                if(swarm[sender].status_rf_command_forward == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_forward = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_forward = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_forward = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 2: {
+                if(swarm[sender].status_rf_command_backward == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_backward = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_backward = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_backward = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 3: {
+                if(swarm[sender].status_rf_command_left == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_left = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_left = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_left = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 4: {
+                if(swarm[sender].status_rf_command_right == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_right = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_right = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_right = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 5: {
+                if(swarm[sender].status_rf_command_left_motor == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_left_motor = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_left_motor = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_left_motor = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 6: {
+                if(swarm[sender].status_rf_command_right_motor == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_right_motor = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_right_motor = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_right_motor = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 7: {
+                if(swarm[sender].status_rf_command_oled_colour == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_oled_colour = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_oled_colour = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_oled_colour = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 8: {
+                if(swarm[sender].status_rf_command_cled_colour == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_cled_colour = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_cled_colour = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_cled_colour = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 9: {
+                if(swarm[sender].status_rf_command_oled_state == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_oled_state = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_oled_state = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_oled_state = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 10: {
+                if(swarm[sender].status_rf_command_cled_state == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_cled_state = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_cled_state = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_cled_state = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 11: {
+                if(swarm[sender].status_rf_command_set_oled == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_set_oled = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_set_oled = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_set_oled = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 12: {
+                if(swarm[sender].status_rf_command_play_tune == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_play_tune = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_play_tune = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_play_tune = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+            case 14: {
+                if(swarm[sender].status_rf_command_sync_time == 1) {
+                    if(success == 1){
+                        if(length == 0) {
+                            swarm[sender].status_rf_command_sync_time = 2;
+                            outcome = 1;
+                        } else {
+                            swarm[sender].status_rf_command_sync_time = 3;
+                            outcome = 3;
+                        }
+                    } else {
+                        swarm[sender].status_rf_command_sync_time = 4;
+                        outcome = 4;
+                    }
+                } else outcome = 2;
+            }
+            break;
+        }
+    }
+
+    if(RF_DEBUG) {
+        switch(outcome) {
+            case 0 :
+                pc.printf("Unknown RF response received");
+            case 1 :
+                pc.printf("RF response received, data updated.");
+            case 2 :
+                pc.printf("Unexpected RF response received, ignored.");
+            case 3 :
+                pc.printf("Invalid RF response received, ignored.");
+            case 4 :
+                pc.printf("RF response received: unsuccessful operation.");
+        }
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r d63a63feb104 communications.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/communications.h	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,115 @@
+/*******************************************************************************************
+ *
+ * University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * Version 0.5  February 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
+ *
+ ******************************************************************************************/
+
+#ifndef COMMUNICATIONS_H
+#define COMMUNICATIONS_H
+
+// The swarm_member struct is used by the communication stack to store the status and results for RF commands and requests
+// that have been sent.  An array of [SWARM_SIZE] swarm_members is created.  This contains a status flag for all of the 
+// standard requests and commands and a set of variables for data which can be collected.  
+//
+// The status flags are set to the following values:
+// 0 : "unused"  - before any requests are sent
+// 1 : "waiting" - a request or command has been sent and the result is not yet known
+// 2 : "received" - a valid response to a request or command has been received
+// 3 : "invalid" - an invalid response to a request or command has been received 
+struct swarm_member {
+    char status_rf_request_null;
+    char status_rf_request_left_motor_speed;
+    char status_rf_request_right_motor_speed;
+    char status_rf_request_button_state;
+    char status_rf_request_led_colour;
+    char status_rf_request_led_states;
+    char status_rf_request_battery;
+    char status_rf_request_light_sensor;
+    char status_rf_request_accelerometer;
+    char status_rf_request_gyroscope;
+    char status_rf_request_background_ir;
+    char status_rf_request_reflected_ir;
+    char status_rf_request_distance_ir;
+    char status_rf_request_line_following_ir;
+    char status_rf_request_uptime;
+    char status_rf_command_stop;
+    char status_rf_command_forward;
+    char status_rf_command_backward;
+    char status_rf_command_left;
+    char status_rf_command_right;
+    char status_rf_command_left_motor;
+    char status_rf_command_right_motor;
+    char status_rf_command_oled_colour;
+    char status_rf_command_cled_colour;
+    char status_rf_command_oled_state;
+    char status_rf_command_cled_state;
+    char status_rf_command_set_oled;
+    char status_rf_command_play_tune;
+    char status_rf_command_sync_time;
+    float left_motor_speed;
+    float right_motor_speed;
+    char button_state;
+    char outer_led_colour [3];
+    char center_led_colour [3];
+    char led_states[2];
+    float battery;
+    float light_sensor;
+    float accelerometer[3];
+    float gyro;
+    unsigned short background_ir[8];
+    unsigned short reflected_ir[8];
+    float distance_ir[8];
+};
+
+void send_rf_message(char target, char command, char * data, char length);
+void setup_communications(void);
+void decodeMessage(char sender, char target, char id, char command, char * data, char length);
+void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length);
+void tdma_response();
+void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length);
+void handle_request(char sender, char is_broadcast, char request_response, char id, char function);
+void processRadioData(char * data, char length);
+void errormessage(int index);
+void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length);
+
+void send_rf_request_null ( char target );
+void send_rf_request_left_motor_speed ( char target );
+void send_rf_request_right_motor_speed ( char target );
+void send_rf_request_button_state ( char target );
+void send_rf_request_led_colour ( char target );
+void send_rf_request_led_states ( char target );
+void send_rf_request_battery ( char target );
+void send_rf_request_light_sensor ( char target );
+void send_rf_request_accelerometer ( char target );
+void send_rf_request_gyroscope ( char target );
+void send_rf_request_background_ir ( char target );
+void send_rf_request_reflected_ir ( char target );
+void send_rf_request_distance_ir ( char target );
+void send_rf_request_line_following_ir ( char target );
+void send_rf_request_uptime ( char target );
+void send_rf_command_stop ( char target, char request_response );
+void send_rf_command_forward ( char target, char request_response, float speed );
+void send_rf_command_backward ( char target, char request_response, float speed );
+void send_rf_command_left ( char target, char request_response, float speed );
+void send_rf_command_right ( char target, char request_response, float speed );
+void send_rf_command_left_motor ( char target, char request_response, float speed );
+void send_rf_command_right_motor ( char target, char request_response, float speed );
+void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue );
+void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue );
+void send_rf_command_oled_state ( char target, char request_response, char led0, char led1, char led2, char led3, char led4, char led5, char led6, char led7, char led8, char led9 );
+void send_rf_command_cled_state ( char target, char request_response, char enable );
+void send_rf_command_set_oled ( char target, char request_response, char oled, char enable );
+void send_rf_command_play_tune ( char target, char request_response, char * data, char length );
+void send_rf_command_sync_time ( char target, char request_response );
+
+
+const char * const requests_array[] = { "Req.Ack", "Req.LM", "Req.RM", "Req.But","Req.LEDCol","Req.LED","Req.Lgt","Req.Acc","Req.Gyro","Req.BIR","Req.RIR","Req.DIR","Req.LFS","Req.UpT","Req.???","Req.???"};
+const char * const commands_array[] = { "Cm.Stop","Cm.Fward","Cm.Bward","Cm.Left","Cm.Right","Cm.LMot","Cm.RMot","Cm.OLEDC","Cm.CLEDC","Cm.OLEDS","Cm.CLEDS","Cm.SetLED","Cm.Play","Cm.Sync","Cm.???","Cm.???"};
+
+#endif //COMMUNICATIONS_H
\ No newline at end of file
diff -r 000000000000 -r d63a63feb104 display.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/display.cpp	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,144 @@
+/* University of York Robotics Laboratory MBED Library: Display Driver
+ * 
+ * File: display.cpp
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * October 2013
+ *
+ * Driver for the Midas 20x4 I2C LCD Display (MCCOG42005A6W-BNMLWI) LCD
+ *
+ * Farnell part 2218946 
+ *
+ */ 
+ 
+#include "mbed.h"
+#include "display.h"
+
+Display::Display(PinName sda, PinName scl, PinName reset) :  Stream("display"), _i2c(sda,scl), _reset(reset)  {
+  
+}
+
+Display::Display() :  Stream("display"), _i2c(p9,p10), _reset(p12)  {
+  
+}
+
+int Display::i2c_message(char byte){
+   char bytes [2];
+   bytes[0]=0x80;
+   bytes[1]=byte;
+   return _i2c.write(LCD_ADDRESS,bytes,2);  
+}
+
+int Display::disp_putc(int c){
+   char message [2];
+   message[0]=0x40;
+   message[1]=c;
+   _i2c.write(LCD_ADDRESS,message,2);
+   return c;
+}
+
+
+
+void Display::init_display(){
+   //Set initial states: display on, cursor off
+   display_on = 1;
+   cursor_on = 1;
+   blink_on  = 1;
+    
+   _reset=1;
+   wait(0.02);
+   //Set reset low
+   _reset=0;
+   wait(0.001);
+   _reset=1;
+   wait(0.03);
+   i2c_message(0x3a); 
+   i2c_message(0x1e); 
+   i2c_message(0x39); 
+   i2c_message(0x1c); 
+   i2c_message(0x79); 
+   i2c_message(0x5d); 
+   i2c_message(0x6d); 
+   _set_display();
+   clear_display(); 
+} 
+
+void Display::write_string(char * message, char length){
+   char to_send [length+1];
+   to_send[0]=0x40;
+   for(int i=0;i<length;i++){
+     to_send[i+1] = message[i];
+   }
+   _i2c.write(LCD_ADDRESS,to_send,length+1);
+}
+
+void Display::set_position(char row, char column){
+  if(row < 4 && column < 20){
+    char pos = 128 +((row * 32)+column);
+    i2c_message(pos);
+  }
+}
+
+void Display::set_cursor(char enable){
+  cursor_on=enable;
+  _set_display();
+}
+
+void Display::set_blink(char enable){
+  blink_on=enable;
+    _set_display();
+}
+
+void Display::set_display(char enable){
+  display_on=enable;
+    _set_display();
+}
+
+void Display::clear_display(){
+  i2c_message(0x01);
+}
+
+void Display::home(){
+  i2c_message(0x02);
+}
+
+
+void Display::_set_display(){
+    char mode = 8;
+    if(display_on>0) mode += 4;
+    if(cursor_on>0) mode += 2;
+    if(blink_on>0) mode ++;
+    i2c_message(mode);
+}
+
+
+int Display::_putc (int c) {
+    disp_putc(c);
+    return(c);
+}
+
+int Display::_getc (void) {
+    char r = 0;
+    return(r);
+}
+ 
+ 
+/* Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
\ No newline at end of file
diff -r 000000000000 -r d63a63feb104 display.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/display.h	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,113 @@
+/* University of York Robotics Laboratory MBED Library: Display Driver
+ * 
+ * File: display.h
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * October 2013
+ *
+ * Driver for the Midas 20x4 I2C LCD Display (MCCOG42005A6W-BNMLWI) LCD
+ *
+ * Farnell part 2218946 
+ *
+ */ 
+ 
+#ifndef DISPLAY_H
+#define DISPLAY_H
+
+//
+// Defines
+//
+
+#define LCD_ADDRESS 0x78
+
+class Display : public Stream {
+
+// Public Functions
+
+public:
+
+   /** Create the LCD Display object connected to the default pins
+     *
+     * @param sda pin   - default is p9
+     * @param scl pin   - default is p10
+     * @param reset pin - default is p12
+     */
+     
+    Display();
+    
+    /** Create the LCD Display object connected to specific pins
+     *
+     */
+    Display(PinName sda, PinName scl, PinName reset);
+
+//Print string message of given length
+void write_string(char * message, char length);
+
+//Set the row and column of cursor position
+void set_position(char row, char column);
+
+// Enable or disable cursor
+void set_cursor(char enable);
+
+// Enable or disable cursor blink
+void set_blink(char enable);
+
+// Enable or disable display
+void set_display(char enable);
+
+
+// Clear display
+void clear_display();
+
+//Set cursor to home position
+void home();
+
+
+// Send a 1-byte control message to the display
+int i2c_message(char byte);
+
+// Default initialisation sequence for the display
+void init_display();
+
+int disp_putc(int c);
+
+
+private :
+
+    I2C _i2c;
+    DigitalOut _reset;
+    
+    char display_on;
+    char cursor_on;
+    char blink_on;
+    
+    void _set_display();
+    
+    virtual int _putc(int c);
+    virtual int _getc();
+    
+};
+
+#endif // DISPLAY_H
+
+ 
+ 
+/* Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
\ No newline at end of file
diff -r 000000000000 -r d63a63feb104 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,192 @@
+/*******************************************************************************************
+ *
+ * University of York Robot Lab Pi Swarm Handheld Controller Software
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * Version 0.5  February 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
+ *
+ ******************************************************************************************/
+
+#include "mbed.h"
+#include "main.h"
+#include "communications.h"
+#include "display.h"
+
+Serial pc (USBTX, USBRX);
+DigitalOut myled(LED1);
+Display display;
+Alpha433 rf;
+Ticker switch_ticker;
+DigitalIn switch_up(p21);
+DigitalIn switch_upright(p22);
+DigitalIn switch_right(p23);
+DigitalIn switch_downright(p24);
+DigitalIn switch_down(p25);
+DigitalIn switch_downleft(p26);
+DigitalIn switch_left(p27);
+DigitalIn switch_upleft(p29);
+DigitalIn switch_center(p28);
+DigitalOut tx_led(p18);
+DigitalOut rx_led(p17);
+char target_id = 0;
+int switch_state = 0;
+int speed_mode = 0;
+float speed = 0.1;
+
+void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
+    // A 'user' RF Command has been received:  write the code here to process it
+    // sender = ID of the sender, range 0 to 31
+    // broadcast_message = 1 is message sent to all robots, 0 otherwise
+    // request_response = 1 if a response is expected, 0 otherwise
+    // id = Message ID, range 0 to 255
+    // is_command = 1 is message is a command, 0 if it is a request.  If RF_ALLOW_COMMANDS is not selected, only requests will be sent to this block
+    // function = The function identifier.  Range 0 to 15
+    // * data = Array containing extra data bytes
+    // length = Length of extra data bytes held (range 0 to 57)
+    
+}    
+
+void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
+    // A 'user' RF Response has been received:  write the code here to process it
+    // sender = ID of the sender, range 0 to 31
+    // broadcast_message = 1 is message sent to all robots, 0 otherwise
+    // success = 1 if operation successful, 0 otherwise
+    // id = Message ID, range 0 to 255
+    // is_command = 1 is message is a command, 0 if it is a request.  If RF_ALLOW_COMMANDS is not selected, only requests will be sent to this block
+    // function = The function identifier.  Range 0 to 15
+    // * data = Array containing extra data bytes
+    // length = Length of extra data bytes held (range 0 to 57)
+  
+}    
+
+
+void processRawRFData(char * rstring, char cCount){
+    // A raw RF packet has been received: write the code here to process it
+    // rstring = The received packet
+    // cCount = Packet length
+}
+
+void check_switch()
+{
+    int new_state = 0;
+    if(!switch_up) new_state = 1;
+    if(!switch_upright) new_state = 2;
+    if(!switch_right) new_state = 3;
+    if(!switch_downright) new_state=4;
+    if(!switch_down) new_state=5;
+    if(!switch_downleft) new_state=6;
+    if(!switch_left) new_state=7;
+    if(!switch_upleft) new_state=8;
+    if(!switch_center) new_state=9;
+    if(new_state!=switch_state) {
+        switch_state=new_state;
+        display.set_position(0,8);
+        switch(switch_state) {
+            case 0:
+                display.printf("          ");
+                break;
+            case 1:
+                display.printf("UP        ");
+                break;
+            case 2:
+                display.printf("UP-RIGHT  ");
+                break;
+            case 3:
+                display.printf("RIGHT     ");
+                break;
+            case 4:
+                display.printf("DOWN-RIGHT");
+                break;
+            case 5:
+                display.printf("DOWN      ");
+                break;
+            case 6:
+                display.printf("DOWN-LEFT ");
+                break;
+            case 7:
+                display.printf("LEFT      ");
+                break;
+            case 8:
+                display.printf("UP-LEFT   ");
+                break;
+            case 9:
+                display.printf("CENTER    ");
+                speed_mode ++;
+                if (speed_mode == 4) speed_mode = 0;
+                switch(speed_mode){
+                    case 0: speed = 0.1;break;
+                    case 1: speed = 0.2;break;
+                    case 2: speed = 0.5;break;
+                    case 3: speed = 1.0;break;   
+                }
+                display.set_position(1,0);
+                display.printf("Speed: %.1f",speed);
+                break;
+        }
+        transmit_message();
+    }
+}
+
+void transmit_message()
+{
+    tx_led = 1;
+    switch(switch_state){
+        case 0: send_rf_command_stop(target_id,1);break;
+        case 1: send_rf_command_forward(target_id,1,speed);break;
+        case 5: send_rf_command_backward(target_id,1,speed);break;
+        case 7: send_rf_command_left(target_id,1,speed);break;
+        case 3: send_rf_command_right(target_id,1,speed);break;
+        case 9: send_rf_message(target_id,0x32,"Hello",5);break;
+    }
+    wait(0.1);
+    tx_led=0;
+}
+
+int main()
+{
+    pc.baud(PC_BAUD);
+    display.init_display();
+    display.printf("YORK ROBOTICS LAB   PI-SWARM CONTROLLER Software 0.5");
+    wait(2.0);
+    display.clear_display();
+    display.printf("Command:");
+    rf.rf_init();
+    rf.setFrequency(435000000);
+    rf.setDatarate(57600);
+    setup_switches();
+    switch_ticker.attach( &check_switch , 0.05 );
+    while(1) {
+        myled = 1;
+        // rx_led=1;
+        // tx_led=0;
+        wait(0.5);
+        // rx_led=0;
+        // tx_led=1;
+        myled = 0;
+        wait(0.5);
+    }
+
+}
+
+void setup_switches()
+{
+    switch_center.mode(PullUp);
+    switch_up.mode(PullUp);
+    switch_upleft.mode(PullUp);
+    switch_left.mode(PullUp);
+    switch_downleft.mode(PullUp);
+    switch_down.mode(PullUp);
+    switch_downright.mode(PullUp);
+    switch_right.mode(PullUp);
+    switch_upright.mode(PullUp);
+}
+
+
+void handleData(char * data, char length)
+{
+    display.set_position(1,1);
+    display.write_string(data,length);
+}
diff -r 000000000000 -r d63a63feb104 main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,31 @@
+#ifndef MAIN_H
+#define MAIN_H
+
+#include "alpha433.h"
+
+#define SWARM_SIZE 32
+#define USE_COMMUNICATION_STACK 1
+#define PC_BAUD 115200
+#define RF_ALLOW_COMMANDS 1
+#define RF_USE_LEDS 1
+#define RF_USE_TDMA 1
+#define RF_TDMA_TIME_PERIOD_US 15625
+#define RF_DEBUG 1
+#define RF_VERBOSE 1
+#define START_RADIO_ON_BOOT 1
+#define RF_FREQUENCY 435000000
+#define RF_DATARATE 57600
+
+extern Serial pc;
+extern Alpha433 rf;
+void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length);
+void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length);
+ 
+void processRawRFData(char * rstring, char cCount);
+void check_switch(void);
+void setup_switches(void);
+void transmit_message(void);
+void handleData(char * data, char length);
+
+
+#endif // MAIN_H
diff -r 000000000000 -r d63a63feb104 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file