James Hilder
Unfinished v0.7, added bearing detection
Fork of
Pi_Swarm_Library_v06_alpha
by 
piswarm
communications.cpp
- Committer:
- jah128
- Date:
- 2014-06-30
- Revision:
- 11:5ebcb52726cf
- Parent:
- 9:7a4fc1d7e484
File content as of revision 11:5ebcb52726cf:
/*******************************************************************************************
*
* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
*
* (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
*
* Version 0.7 May 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 "communications.h"
#include "piswarm.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 [4+length];
message[0]=piswarm.get_id() + 32;
message[1]=target + 32;
message_id++;
message[2]=message_id % 256;
message[3]=command;
for(int i=0; i<length; i++) {
message[4+i]=data[i];
}
piswarm.send_rf_message(message,4+length);
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 R:%i U:%i C:%x{%s} L:%i", sender, target, id, is_response, is_user, command, commands_array[function],length);
else pc.printf("Message: S:%i T:%i ID:%x R:%i U:%i C:%x{%s} L:%i", sender, target, id, is_response, is_user, command, requests_array[function],length);
}
//Action the message only if I am a recipient
if(target==0 || target==piswarm.get_id()) {
if(target==0)gi_RF_received_broadcast++;
else gi_RF_received_targeted_to_me++;
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 {
gi_RF_received_not_for_me++;
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]=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 {
gi_RF_responses_sent++;
piswarm.send_rf_message(message,4+length);
if(RF_DEBUG==1)pc.printf("Response issued");
}
}
// Send a delayed response
void tdma_response()
{
gi_RF_responses_sent++;
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 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(success == 1){
gi_RF_commands_actioned++;
}else gi_RF_commands_invalid++;
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 = 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();
success=1;
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);
success=1;
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;
success=1;
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;
success=1;
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;
success=1;
break;
}
case 8: { // Request accelerometer reading
response_length = 6;
response = new char[6];
int acc_x = (int)piswarm.read_accelerometer_x();
int acc_y = (int)piswarm.read_accelerometer_y();
int acc_z = (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;
success=1;
break;
}
case 9: { // Request gyroscope reading
response_length = 2;
response = new char[2];
float fgyro = piswarm.read_gyro();
fgyro += 32768;
unsigned short sgyro = (unsigned short) fgyro;
response[0] = sgyro >> 8;
response[1] = sgyro % 256;
success=1;
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 = piswarm.read_adc_value(sensor);
response[offset]=m_val >> 8;
response[offset+1]=m_val % 256;
}
success=1;
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 = piswarm.read_illuminated_raw_ir_value(sensor);
response[offset]=m_val >> 8;
response[offset+1]=m_val % 256;
}
success=1;
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 = 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;
}
success=1;
break;
}
case 13: // Request line-tracking IR reading
break;
case 14: // Request uptime
break;
case 15: //
break;
}
if(success==1){
gi_RF_requests_actioned++;
}
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
gi_RF_received_invalid_format++;
if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
break;
case 1: //Sender out of valid range
gi_RF_received_invalid_sender++;
if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
break;
case 2: //Target out of valid range
gi_RF_received_invalid_target++;
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 = 0x40;
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 = 0x41;
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 = 0x42;
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 = 0x43;
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 = 0x44;
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 = 0x45;
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 = 0x46;
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 = 0x47;
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 = 0x48;
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 = 0x49;
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 = 0x4A;
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 = 0x4B;
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 = 0x4C;
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 = 0x4D;
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 = 0x4E;
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 = NULL;
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");
break;
case 1 :
pc.printf("RF response received, data updated.");
break;
case 2 :
pc.printf("Unexpected RF response received, ignored.");
break;
case 3 :
pc.printf("Invalid RF response received, ignored.");
break;
case 4 :
pc.printf("RF response received: unsuccessful operation.");
break;
}
}
}