James Hilder
V0.5 library for the Pi Swarm robot
Diff: communications.cpp
- Revision:
- 0:9ffe8ebd1c40
- Child:
- 1:b067a08ff54e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/communications.cpp Fri Jan 31 22:59:25 2014 +0000
@@ -0,0 +1,382 @@
+/* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * Version 0.4 January 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
+ *
+ */
+
+ #include "communications.h"
+ #include "piswarm.h"
+ #include "main.h"
+
+ 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;
+
+ 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.???"};
+
+//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 || target==piswarm.get_id()){
+ 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 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]=piswarm.get_id();
+ 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];
+ }
+ //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
+ {
+ piswarm.send_rf_message(message,4+length);
+ if(RF_DEBUG==1)pc.printf("Response issued");
+ }
+}
+
+// Send a delayed response
+void tdma_response(){
+ piswarm.send_rf_message(waiting_message,4+waiting_length);
+ tdma_busy = 0;
+ if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
+}
+
+// Handle a message that is a response
+void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
+
+}
+
+// Handle a message that is a (not-user) 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 (not-user) 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 = 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 = 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]=piswarm.get_switches();
+ break;
+ }
+ case 4: // Request LED colours
+ {
+ response_length = 6;
+ response = new char[6];
+ int oled_colour = piswarm.get_oled_colour();
+ int cled_colour = 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
+ break;
+ case 6: // Request battery power
+ {
+ response_length = 2;
+ response = new char[2];
+ float fbattery = 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 = 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
+ break;
+ case 9: // Request gyroscope reading
+ break;
+ case 10: // Request background IR reading
+ break;
+ case 11: // Request illuminated IR reading
+ break;
+ case 12: // Request line-tracking IR reading
+ break;
+ case 13: // Request uptime
+ break;
+ case 14: //
+ break;
+ case 15: //
+ break;
+ }
+ send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
+
+}
+
+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;
+}
+
+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;
+ }
+}
\ No newline at end of file