James Hilder
Handheld controller (RF) for Pi Swarm system
Fork of
Pi_Swarm_Handheld_Controller
by 
piswarm
Revision 0:d63a63feb104, committed 2014-06-10
- Comitter:
- jah128
- Date:
- Tue Jun 10 11:05:23 2014 +0000
- Commit message:
- Handheld controller for Pi Swarm (old code)
Changed in this revision
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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);
+}
--- /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
--- /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