Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
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