Basic code for the Pi Swarm Table Controller
Dependencies: mbed
Fork of Programmable_IR_Beacon by
Code for the Pi Swarm Table Controller (PCB 1.0)
Diff: communications.cpp
- Revision:
- 2:c81f4ef63132
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/communications.cpp Thu May 22 14:25:10 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