PiSwarmTableController - Table controller for piswarm-office

Users » hee502 » Code » PiSwarmTableController

Huseyin Erdem / Mbed 2 deprecated PiSwarmTableController

Table controller for piswarm-office

Fork of PiSwarmTableController by piswarm

communications.cpp@5:68a1ce96bfeb, 2014-08-14 (annotated)

Committer:: hee502
Date:: Thu Aug 14 09:06:31 2014 +0000
Revision:: 5:68a1ce96bfeb
Parent:: 2:c81f4ef63132

Table controller for piswarm-office

Who changed what in which revision?

User	Revision	Line number	New contents of line
jah128	2:c81f4ef63132	1	/*******************************************************************************************
jah128	2:c81f4ef63132	2	*
jah128	2:c81f4ef63132	3	* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
jah128	2:c81f4ef63132	4	*
jah128	2:c81f4ef63132	5	* (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128	2:c81f4ef63132	6	*
jah128	2:c81f4ef63132	7	* Version 0.5 February 2014
jah128	2:c81f4ef63132	8	*
jah128	2:c81f4ef63132	9	* Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
jah128	2:c81f4ef63132	10	*
jah128	2:c81f4ef63132	11	******************************************************************************************/
jah128	2:c81f4ef63132	12
jah128	2:c81f4ef63132	13	// Important note: The communication stack is enabled by setting the "USE_COMMUNICATION_STACK" flag to 1
jah128	2:c81f4ef63132	14	// When being used, all received messages are decoded using the decodeMessage() function
jah128	2:c81f4ef63132	15	// See manual for more info on using the communication handler
jah128	2:c81f4ef63132	16	#include "mbed.h"
jah128	2:c81f4ef63132	17	#include "communications.h"
jah128	2:c81f4ef63132	18	#include "main.h"
jah128	2:c81f4ef63132	19
jah128	2:c81f4ef63132	20	struct swarm_member swarm[SWARM_SIZE]; // Array to hold received information about other swarm members
jah128	2:c81f4ef63132	21	DigitalOut actioning (LED1);
jah128	2:c81f4ef63132	22	DigitalOut errorled (LED2);
jah128	2:c81f4ef63132	23	DigitalOut tx (LED3);
jah128	2:c81f4ef63132	24	DigitalOut rx (LED4);
jah128	2:c81f4ef63132	25	Timeout tdma_timeout;
jah128	2:c81f4ef63132	26	char tdma_busy = 0;
jah128	2:c81f4ef63132	27	char waiting_message [64];
jah128	2:c81f4ef63132	28	char waiting_length = 0;
jah128	2:c81f4ef63132	29	int message_id = 0;
jah128	2:c81f4ef63132	30
jah128	2:c81f4ef63132	31
jah128	2:c81f4ef63132	32	// Send a structured message over the RF interface
jah128	2:c81f4ef63132	33	// @target - The target recipient (1-31 or 0 for broadcast)
jah128	2:c81f4ef63132	34	// @command - The command byte (see manual)
jah128	2:c81f4ef63132	35	// @*data - Additional data bytes
jah128	2:c81f4ef63132	36	// @length - Length of additional data
jah128	2:c81f4ef63132	37	void send_rf_message(char target, char command, char * data, char length)
jah128	2:c81f4ef63132	38	{
jah128	2:c81f4ef63132	39	char message [5+length];
jah128	2:c81f4ef63132	40	message_id++;
jah128	2:c81f4ef63132	41	message[0] = 32;
jah128	2:c81f4ef63132	42	message[1] = target+32;
jah128	2:c81f4ef63132	43	message[2] = message_id%256;
jah128	2:c81f4ef63132	44	message[3] = command;
jah128	2:c81f4ef63132	45	for(int i=0;i<length;i++){
jah128	2:c81f4ef63132	46	message[4+i] = data[i];
jah128	2:c81f4ef63132	47	}
jah128	2:c81f4ef63132	48	message[4+length]=NULL;
jah128	2:c81f4ef63132	49	rf.sendString(4+length,message);
hee502	5:68a1ce96bfeb	50	//if(RF_DEBUG==1)//pc.printf("RF message sent");
jah128	2:c81f4ef63132	51	}
jah128	2:c81f4ef63132	52
jah128	2:c81f4ef63132	53
jah128	2:c81f4ef63132	54	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	2:c81f4ef63132	55	// Internal Functions
jah128	2:c81f4ef63132	56	// In general these functions should not be called by user code
jah128	2:c81f4ef63132	57	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	2:c81f4ef63132	58
jah128	2:c81f4ef63132	59	// Decode the received message header. Check it is min. 4 bytes long, that the sender and target are valid [called in alpha433.cpp]
jah128	2:c81f4ef63132	60	void processRadioData(char * data, char length)
jah128	2:c81f4ef63132	61	{
jah128	2:c81f4ef63132	62	if(RF_USE_LEDS==1) {
jah128	2:c81f4ef63132	63	errorled=0;
jah128	2:c81f4ef63132	64	rx=1;
jah128	2:c81f4ef63132	65	}
jah128	2:c81f4ef63132	66	// Decompose the received message
jah128	2:c81f4ef63132	67	if(length < 4) errormessage(0);
jah128	2:c81f4ef63132	68	else {
jah128	2:c81f4ef63132	69	// Establish the sender and target of the packet
jah128	2:c81f4ef63132	70	char sender = data[0];
jah128	2:c81f4ef63132	71	char target = data[1];
jah128	2:c81f4ef63132	72	char id = data[2];
jah128	2:c81f4ef63132	73	char command = data[3];
jah128	2:c81f4ef63132	74	if(sender<32 \|\| sender>63)errormessage(1);
jah128	2:c81f4ef63132	75	else {
jah128	2:c81f4ef63132	76	if(target<32 \|\| target>63)errormessage(2);
jah128	2:c81f4ef63132	77	else {
jah128	2:c81f4ef63132	78	sender -= 32;
jah128	2:c81f4ef63132	79	target -= 32;
jah128	2:c81f4ef63132	80	decodeMessage(sender,target,id,command,data+4,length-4);
jah128	2:c81f4ef63132	81	}
jah128	2:c81f4ef63132	82	}
jah128	2:c81f4ef63132	83	}
jah128	2:c81f4ef63132	84	if(RF_USE_LEDS==1) rx=0;
jah128	2:c81f4ef63132	85	}
jah128	2:c81f4ef63132	86
jah128	2:c81f4ef63132	87	//Decode the received message, action it if it is valid and for me
jah128	2:c81f4ef63132	88	void decodeMessage(char sender, char target, char id, char command, char * data, char length)
jah128	2:c81f4ef63132	89	{
jah128	2:c81f4ef63132	90	char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
jah128	2:c81f4ef63132	91
jah128	2:c81f4ef63132	92	if(target==0) broadcast_message = 1;
jah128	2:c81f4ef63132	93	is_response = 0 != (command & (1 << 7));
jah128	2:c81f4ef63132	94	request_response = 0 != (command & (1 << 6));
jah128	2:c81f4ef63132	95	is_user = 0 != (command & (1 << 5));
jah128	2:c81f4ef63132	96	is_command = 0 != (command & (1 << 4));
jah128	2:c81f4ef63132	97	function = command % 16;
jah128	2:c81f4ef63132	98
jah128	2:c81f4ef63132	99	if (RF_DEBUG==1) {
jah128	2:c81f4ef63132	100	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);
jah128	2:c81f4ef63132	101	else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length);
jah128	2:c81f4ef63132	102	}
jah128	2:c81f4ef63132	103
jah128	2:c81f4ef63132	104	//Action the message only if I am a recipient
jah128	2:c81f4ef63132	105	if(target==0) {
jah128	2:c81f4ef63132	106	if(RF_USE_LEDS==1) actioning = 1;
jah128	2:c81f4ef63132	107	if(is_response == 1) {
jah128	2:c81f4ef63132	108	if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128	2:c81f4ef63132	109	else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128	2:c81f4ef63132	110	} else {
jah128	2:c81f4ef63132	111	if(is_command == 1) {
jah128	2:c81f4ef63132	112	if(RF_ALLOW_COMMANDS == 1) {
jah128	2:c81f4ef63132	113	if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128	2:c81f4ef63132	114	else handle_command(sender, broadcast_message, request_response, id, function, data, length);
jah128	2:c81f4ef63132	115	} else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
jah128	2:c81f4ef63132	116	} else {
jah128	2:c81f4ef63132	117	//A information request has no extra parameters
jah128	2:c81f4ef63132	118	if(length == 0) {
jah128	2:c81f4ef63132	119	if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128	2:c81f4ef63132	120	else handle_request(sender, broadcast_message, request_response, id, function);
jah128	2:c81f4ef63132	121	} else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	122	}
jah128	2:c81f4ef63132	123	}
jah128	2:c81f4ef63132	124	if(RF_USE_LEDS==1) actioning = 0;
jah128	2:c81f4ef63132	125	} else if (RF_DEBUG==1) pc.printf(" - Ignored\n");
jah128	2:c81f4ef63132	126	}
jah128	2:c81f4ef63132	127
jah128	2:c81f4ef63132	128	//Send a predefined response message
jah128	2:c81f4ef63132	129	void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
jah128	2:c81f4ef63132	130	{
jah128	2:c81f4ef63132	131	char message [4+length];
jah128	2:c81f4ef63132	132	message[0]=0;
jah128	2:c81f4ef63132	133	message[1]=target;
jah128	2:c81f4ef63132	134	message[2]=id;
jah128	2:c81f4ef63132	135	message[3]=128 + (success << 6) + (is_command << 4) + function;
jah128	2:c81f4ef63132	136	for(int i=0; i<length; i++) {
jah128	2:c81f4ef63132	137	message[4+i]=data[i];
jah128	2:c81f4ef63132	138	}
jah128	2:c81f4ef63132	139	/*TDMA not relevant to handheld controller
jah128	2:c81f4ef63132	140	//Delay the response if it is broadcast and TDMA mode is on
jah128	2:c81f4ef63132	141	if(RF_USE_TDMA == 1 && is_broadcast == 1) {
jah128	2:c81f4ef63132	142	if(tdma_busy == 1) {
jah128	2:c81f4ef63132	143	if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
jah128	2:c81f4ef63132	144	} else {
jah128	2:c81f4ef63132	145	tdma_busy = 1;
jah128	2:c81f4ef63132	146	strcpy(waiting_message,message);
jah128	2:c81f4ef63132	147	waiting_length=length;
jah128	2:c81f4ef63132	148	tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
jah128	2:c81f4ef63132	149	if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
jah128	2:c81f4ef63132	150	}
jah128	2:c81f4ef63132	151	} else {
jah128	2:c81f4ef63132	152	*/
jah128	2:c81f4ef63132	153	rf.sendString(4+length,message);
jah128	2:c81f4ef63132	154	if(RF_DEBUG==1)pc.printf("Response issued");
jah128	2:c81f4ef63132	155	//}
jah128	2:c81f4ef63132	156	}
jah128	2:c81f4ef63132	157
jah128	2:c81f4ef63132	158	// Send a delayed response
jah128	2:c81f4ef63132	159	void tdma_response()
jah128	2:c81f4ef63132	160	{
jah128	2:c81f4ef63132	161	rf.sendString(4+waiting_length,waiting_message);
jah128	2:c81f4ef63132	162	tdma_busy = 0;
jah128	2:c81f4ef63132	163	if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
jah128	2:c81f4ef63132	164	}
jah128	2:c81f4ef63132	165
jah128	2:c81f4ef63132	166	// Handle a message that is a predefined command
jah128	2:c81f4ef63132	167	void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length)
jah128	2:c81f4ef63132	168	{
jah128	2:c81f4ef63132	169	char success = 0;
jah128	2:c81f4ef63132	170	switch(function) {
jah128	2:c81f4ef63132	171	case 0: // Stop [0 data]
jah128	2:c81f4ef63132	172	if(length==0) {
jah128	2:c81f4ef63132	173	//piswarm.stop();
hee502	5:68a1ce96bfeb	174	if(RF_DEBUG==1) //pc.printf(" - Stop Command Issued - ");
jah128	2:c81f4ef63132	175	success = 1;
jah128	2:c81f4ef63132	176	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	177	break;
jah128	2:c81f4ef63132	178	case 1: // Forward [2 bytes: 16-bit signed short]
jah128	2:c81f4ef63132	179	if(length==2) {
jah128	2:c81f4ef63132	180	int i_speed = (data[0] << 8) + data[1];
jah128	2:c81f4ef63132	181	float speed = i_speed / 32768.0;
jah128	2:c81f4ef63132	182	speed--;
jah128	2:c81f4ef63132	183	//piswarm.forward(speed);
jah128	2:c81f4ef63132	184	success = 1;
jah128	2:c81f4ef63132	185	if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
jah128	2:c81f4ef63132	186	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	187	break;
jah128	2:c81f4ef63132	188	case 2: // Backward [2 bytes: 16-bit signed short]
jah128	2:c81f4ef63132	189	if(length==2) {
jah128	2:c81f4ef63132	190	int i_speed = (data[0] << 8) + data[1];
jah128	2:c81f4ef63132	191	float speed = i_speed / 32768.0;
jah128	2:c81f4ef63132	192	speed--;
jah128	2:c81f4ef63132	193	//piswarm.backward(speed);
jah128	2:c81f4ef63132	194	success = 1;
jah128	2:c81f4ef63132	195	if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
jah128	2:c81f4ef63132	196	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	197	break;
jah128	2:c81f4ef63132	198	case 3: // Left [2 bytes: 16-bit signed short]
jah128	2:c81f4ef63132	199	if(length==2) {
jah128	2:c81f4ef63132	200	int i_speed = (data[0] << 8) + data[1];
jah128	2:c81f4ef63132	201	float speed = i_speed / 32768.0;
jah128	2:c81f4ef63132	202	speed--;
jah128	2:c81f4ef63132	203	//piswarm.left(speed);
jah128	2:c81f4ef63132	204	success = 1;
jah128	2:c81f4ef63132	205	if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
jah128	2:c81f4ef63132	206	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	207	break;
jah128	2:c81f4ef63132	208	case 4: // Right [2 bytes: 16-bit signed short]
jah128	2:c81f4ef63132	209	if(length==2) {
jah128	2:c81f4ef63132	210	int i_speed = (data[0] << 8) + data[1];
jah128	2:c81f4ef63132	211	float speed = i_speed / 32768.0;
jah128	2:c81f4ef63132	212	speed--;
jah128	2:c81f4ef63132	213	//piswarm.right(speed);
jah128	2:c81f4ef63132	214	success = 1;
jah128	2:c81f4ef63132	215	if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
jah128	2:c81f4ef63132	216	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	217	break;
jah128	2:c81f4ef63132	218	case 5: // Left Motor [2 bytes: 16-bit signed short]
jah128	2:c81f4ef63132	219	if(length==2) {
jah128	2:c81f4ef63132	220	int i_speed = (data[0] << 8) + data[1];
jah128	2:c81f4ef63132	221	float speed = i_speed / 32768.0;
jah128	2:c81f4ef63132	222	speed--;
jah128	2:c81f4ef63132	223	//piswarm.left_motor(speed);
jah128	2:c81f4ef63132	224	success = 1;
jah128	2:c81f4ef63132	225	if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
jah128	2:c81f4ef63132	226	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	227	break;
jah128	2:c81f4ef63132	228	case 6: // Right Motor [2 bytes: 16-bit signed short]
jah128	2:c81f4ef63132	229	if(length==2) {
jah128	2:c81f4ef63132	230	int i_speed = (data[0] << 8) + data[1];
jah128	2:c81f4ef63132	231	float speed = i_speed / 32768.0;
jah128	2:c81f4ef63132	232	speed--;
jah128	2:c81f4ef63132	233	//piswarm.right_motor(speed);
jah128	2:c81f4ef63132	234	success = 1;
jah128	2:c81f4ef63132	235	if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
jah128	2:c81f4ef63132	236	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	237	break;
jah128	2:c81f4ef63132	238	case 7: // Outer LED Colour [3 bytes: R, G, B]
jah128	2:c81f4ef63132	239	if(length==3) {
jah128	2:c81f4ef63132	240	//piswarm.set_oled_colour (data[0],data[1],data[2]);
jah128	2:c81f4ef63132	241	success = 1;
jah128	2:c81f4ef63132	242	if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
jah128	2:c81f4ef63132	243	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	244	break;
jah128	2:c81f4ef63132	245	case 8: // Center LED Colour[3 bytes: R, G, B]
jah128	2:c81f4ef63132	246	if(length==3) {
jah128	2:c81f4ef63132	247	//piswarm.set_cled_colour (data[0],data[1],data[2]);
jah128	2:c81f4ef63132	248	success = 1;
jah128	2:c81f4ef63132	249	if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
jah128	2:c81f4ef63132	250	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	251	break;
jah128	2:c81f4ef63132	252	case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ]
jah128	2:c81f4ef63132	253	if(length==2) {
jah128	2:c81f4ef63132	254	//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)));
jah128	2:c81f4ef63132	255	success = 1;
jah128	2:c81f4ef63132	256	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	257	break;
jah128	2:c81f4ef63132	258	case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
jah128	2:c81f4ef63132	259	if(length==1) {
jah128	2:c81f4ef63132	260	//piswarm.enable_cled (data[0] % 2);
jah128	2:c81f4ef63132	261	success = 1;
jah128	2:c81f4ef63132	262	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	263	break;
jah128	2:c81f4ef63132	264	case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED]
jah128	2:c81f4ef63132	265	if(length==1) {
jah128	2:c81f4ef63132	266	int led = data[0] % 16;
jah128	2:c81f4ef63132	267	if(led < 10) {
jah128	2:c81f4ef63132	268	// piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
jah128	2:c81f4ef63132	269	success = 1;
jah128	2:c81f4ef63132	270	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	271	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	272	break;
jah128	2:c81f4ef63132	273	case 12: // Play sound [Minimum 1 byte]
jah128	2:c81f4ef63132	274	if(length>0) {
jah128	2:c81f4ef63132	275	//piswarm.play_tune(data,length);
jah128	2:c81f4ef63132	276	success = 1;
jah128	2:c81f4ef63132	277	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	278	break;
jah128	2:c81f4ef63132	279	case 13: // Sync time
jah128	2:c81f4ef63132	280	if(length==4) {
jah128	2:c81f4ef63132	281	unsigned int new_time = 0;
jah128	2:c81f4ef63132	282	new_time+=((unsigned int)data[0] << 24);
jah128	2:c81f4ef63132	283	new_time+=((unsigned int)data[1] << 16);
jah128	2:c81f4ef63132	284	new_time+=((unsigned int)data[2] << 8);
jah128	2:c81f4ef63132	285	new_time+=(unsigned int)data[3];
jah128	2:c81f4ef63132	286	set_time(new_time);
jah128	2:c81f4ef63132	287	//display_system_time();
jah128	2:c81f4ef63132	288	} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128	2:c81f4ef63132	289	break;
jah128	2:c81f4ef63132	290	case 14: //
jah128	2:c81f4ef63132	291	break;
jah128	2:c81f4ef63132	292	case 15: //
jah128	2:c81f4ef63132	293	break;
jah128	2:c81f4ef63132	294	}
jah128	2:c81f4ef63132	295	if(request_response == 1) {
jah128	2:c81f4ef63132	296	send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
jah128	2:c81f4ef63132	297	}
jah128	2:c81f4ef63132	298
jah128	2:c81f4ef63132	299	}
jah128	2:c81f4ef63132	300
jah128	2:c81f4ef63132	301	//Handle a message that is a predefined request
jah128	2:c81f4ef63132	302	void handle_request(char sender, char is_broadcast, char request_response, char id, char function)
jah128	2:c81f4ef63132	303	{
jah128	2:c81f4ef63132	304	int response_length = 0;
jah128	2:c81f4ef63132	305	char * response = NULL;
jah128	2:c81f4ef63132	306	char success = 0;
jah128	2:c81f4ef63132	307
jah128	2:c81f4ef63132	308	switch(function) {
jah128	2:c81f4ef63132	309	case 0: // Null request
jah128	2:c81f4ef63132	310	success=1;
jah128	2:c81f4ef63132	311	break;
jah128	2:c81f4ef63132	312	case 1: { // Request left motor speed
jah128	2:c81f4ef63132	313	response_length = 2;
jah128	2:c81f4ef63132	314	float speed = 0;//piswarm.get_left_motor() * 32767;
jah128	2:c81f4ef63132	315	int a_speed = 32768 + (int) speed;
jah128	2:c81f4ef63132	316	char msb = (char) (a_speed / 256);
jah128	2:c81f4ef63132	317	char lsb = (char) (a_speed % 256);
jah128	2:c81f4ef63132	318	response = new char[2];
jah128	2:c81f4ef63132	319	response[0]=msb;
jah128	2:c81f4ef63132	320	response[1]=lsb;
jah128	2:c81f4ef63132	321	success=1;
jah128	2:c81f4ef63132	322	break;
jah128	2:c81f4ef63132	323	}
jah128	2:c81f4ef63132	324	case 2: { // Request right motor speed
jah128	2:c81f4ef63132	325	response_length = 2;
jah128	2:c81f4ef63132	326	float speed = 0;//piswarm.get_right_motor() * 32767;
jah128	2:c81f4ef63132	327	int a_speed = 32768 + (int) speed;
jah128	2:c81f4ef63132	328	char msb = (char) (a_speed / 256);
jah128	2:c81f4ef63132	329	char lsb = (char) (a_speed % 256);
jah128	2:c81f4ef63132	330	response = new char[2];
jah128	2:c81f4ef63132	331	response[0]=msb;
jah128	2:c81f4ef63132	332	response[1]=lsb;
jah128	2:c81f4ef63132	333	success=1;
jah128	2:c81f4ef63132	334	break;
jah128	2:c81f4ef63132	335	}
jah128	2:c81f4ef63132	336	case 3: { // Request button state
jah128	2:c81f4ef63132	337	response_length = 1;
jah128	2:c81f4ef63132	338	response = new char[1];
jah128	2:c81f4ef63132	339	response[0]=0;//piswarm.get_switches();
jah128	2:c81f4ef63132	340	break;
jah128	2:c81f4ef63132	341	}
jah128	2:c81f4ef63132	342	case 4: { // Request LED colours
jah128	2:c81f4ef63132	343	response_length = 6;
jah128	2:c81f4ef63132	344	response = new char[6];
jah128	2:c81f4ef63132	345	int oled_colour = 0;//piswarm.get_oled_colour();
jah128	2:c81f4ef63132	346	int cled_colour = 0;//piswarm.get_cled_colour();
jah128	2:c81f4ef63132	347	response[0] = (char) (oled_colour >> 16);
jah128	2:c81f4ef63132	348	response[1] = (char) ((oled_colour >> 8) % 256);
jah128	2:c81f4ef63132	349	response[2] = (char) (oled_colour % 256);
jah128	2:c81f4ef63132	350	response[3] = (char) (cled_colour >> 16);
jah128	2:c81f4ef63132	351	response[4] = (char) ((cled_colour >> 8) % 256);
jah128	2:c81f4ef63132	352	response[5] = (char) (cled_colour % 256);
jah128	2:c81f4ef63132	353	break;
jah128	2:c81f4ef63132	354	}
jah128	2:c81f4ef63132	355	case 5: { // Request LED states
jah128	2:c81f4ef63132	356	response_length = 2;
jah128	2:c81f4ef63132	357	response = new char[2];
jah128	2:c81f4ef63132	358	response[0] = 0;
jah128	2:c81f4ef63132	359	response[1] = 0;
jah128	2:c81f4ef63132	360	//if (piswarm.get_cled_state() == 1) response[0] += 4;
jah128	2:c81f4ef63132	361	//if (piswarm.get_oled_state(0) == 1) response[0] += 2;
jah128	2:c81f4ef63132	362	//if (piswarm.get_oled_state(1) == 1) response[0] += 1;
jah128	2:c81f4ef63132	363	//if (piswarm.get_oled_state(2) == 1) response[1] += 128;
jah128	2:c81f4ef63132	364	//if (piswarm.get_oled_state(3) == 1) response[1] += 64;
jah128	2:c81f4ef63132	365	//if (piswarm.get_oled_state(4) == 1) response[1] += 32;
jah128	2:c81f4ef63132	366	//if (piswarm.get_oled_state(5) == 1) response[1] += 16;
jah128	2:c81f4ef63132	367	//if (piswarm.get_oled_state(6) == 1) response[1] += 8;
jah128	2:c81f4ef63132	368	//if (piswarm.get_oled_state(7) == 1) response[1] += 4;
jah128	2:c81f4ef63132	369	//if (piswarm.get_oled_state(8) == 1) response[1] += 2;
jah128	2:c81f4ef63132	370	//if (piswarm.get_oled_state(9) == 1) response[1] += 1;
jah128	2:c81f4ef63132	371	break;
jah128	2:c81f4ef63132	372	}
jah128	2:c81f4ef63132	373	case 6: { // Request battery power
jah128	2:c81f4ef63132	374	response_length = 2;
jah128	2:c81f4ef63132	375	response = new char[2];
jah128	2:c81f4ef63132	376	float fbattery = 0;//piswarm.battery() * 1000.0;
jah128	2:c81f4ef63132	377	unsigned short battery = (unsigned short) fbattery;
jah128	2:c81f4ef63132	378	response[0] = battery >> 8;
jah128	2:c81f4ef63132	379	response[1] = battery % 256;
jah128	2:c81f4ef63132	380	break;
jah128	2:c81f4ef63132	381	}
jah128	2:c81f4ef63132	382	case 7: { // Request light sensor reading
jah128	2:c81f4ef63132	383	response_length = 2;
jah128	2:c81f4ef63132	384	response = new char[2];
jah128	2:c81f4ef63132	385	float flight = 0;//piswarm.read_light_sensor() * 655.0;
jah128	2:c81f4ef63132	386	unsigned short light = (unsigned short) flight;
jah128	2:c81f4ef63132	387	response[0] = light >> 8;
jah128	2:c81f4ef63132	388	response[1] = light % 256;
jah128	2:c81f4ef63132	389	break;
jah128	2:c81f4ef63132	390	}
jah128	2:c81f4ef63132	391	case 8: { // Request accelerometer reading
jah128	2:c81f4ef63132	392	response_length = 6;
jah128	2:c81f4ef63132	393	response = new char[6];
jah128	2:c81f4ef63132	394	int acc_x = 0;//(int)piswarm.read_accelerometer_x();
jah128	2:c81f4ef63132	395	int acc_y = 0;//(int)piswarm.read_accelerometer_y();
jah128	2:c81f4ef63132	396	int acc_z = 0;//(int)piswarm.read_accelerometer_z();
jah128	2:c81f4ef63132	397	acc_x += 32768;
jah128	2:c81f4ef63132	398	acc_y += 32768;
jah128	2:c81f4ef63132	399	acc_z += 32768;
jah128	2:c81f4ef63132	400	response[0]=acc_x >> 8;
jah128	2:c81f4ef63132	401	response[1]=acc_x % 256;
jah128	2:c81f4ef63132	402	response[2]=acc_y >> 8;
jah128	2:c81f4ef63132	403	response[3]=acc_y % 256;
jah128	2:c81f4ef63132	404	response[4]=acc_z >> 8;
jah128	2:c81f4ef63132	405	response[5]=acc_z % 256;
jah128	2:c81f4ef63132	406	break;
jah128	2:c81f4ef63132	407	}
jah128	2:c81f4ef63132	408	case 9: { // Request gyroscope reading
jah128	2:c81f4ef63132	409	response_length = 2;
jah128	2:c81f4ef63132	410	response = new char[2];
jah128	2:c81f4ef63132	411	float fgyro = 0;//piswarm.read_gyro();
jah128	2:c81f4ef63132	412	fgyro += 32768;
jah128	2:c81f4ef63132	413	unsigned short sgyro = (unsigned short) fgyro;
jah128	2:c81f4ef63132	414	response[0] = sgyro >> 8;
jah128	2:c81f4ef63132	415	response[1] = sgyro % 256;
jah128	2:c81f4ef63132	416	break;
jah128	2:c81f4ef63132	417	}
jah128	2:c81f4ef63132	418	case 10: { // Request background IR reading
jah128	2:c81f4ef63132	419	response_length = 16;
jah128	2:c81f4ef63132	420	response = new char[16];
jah128	2:c81f4ef63132	421	for(int sensor = 0; sensor < 8; sensor ++){
jah128	2:c81f4ef63132	422	int offset = sensor * 2;
jah128	2:c81f4ef63132	423	unsigned short m_val = 0;//piswarm.read_adc_value(sensor);
jah128	2:c81f4ef63132	424	response[offset]=m_val >> 8;
jah128	2:c81f4ef63132	425	response[offset+1]=m_val % 256;
jah128	2:c81f4ef63132	426	}
jah128	2:c81f4ef63132	427	break;
jah128	2:c81f4ef63132	428	}
jah128	2:c81f4ef63132	429	case 11: { // Request illuminated IR reading
jah128	2:c81f4ef63132	430	response_length = 16;
jah128	2:c81f4ef63132	431	response = new char[16];
jah128	2:c81f4ef63132	432	for(int sensor = 0; sensor < 8; sensor ++){
jah128	2:c81f4ef63132	433	int offset = sensor * 2;
jah128	2:c81f4ef63132	434	unsigned short m_val = 0;//piswarm.read_illuminated_raw_ir_value(sensor);
jah128	2:c81f4ef63132	435	response[offset]=m_val >> 8;
jah128	2:c81f4ef63132	436	response[offset+1]=m_val % 256;
jah128	2:c81f4ef63132	437	}
jah128	2:c81f4ef63132	438	break;
jah128	2:c81f4ef63132	439	}
jah128	2:c81f4ef63132	440
jah128	2:c81f4ef63132	441	case 12: { // Request distance IR reading
jah128	2:c81f4ef63132	442	response_length = 16;
jah128	2:c81f4ef63132	443	response = new char[16];
jah128	2:c81f4ef63132	444	for(int sensor = 0; sensor < 8; sensor ++){
jah128	2:c81f4ef63132	445	int offset = sensor * 2;
jah128	2:c81f4ef63132	446	float f_val = 0;//piswarm.read_reflected_ir_distance(sensor);
jah128	2:c81f4ef63132	447	//f_val ranges from 0 to 100.0;
jah128	2:c81f4ef63132	448	f_val *= 655;
jah128	2:c81f4ef63132	449	unsigned short m_val = (unsigned short) f_val;
jah128	2:c81f4ef63132	450	response[offset]=m_val >> 8;
jah128	2:c81f4ef63132	451	response[offset+1]=m_val % 256;
jah128	2:c81f4ef63132	452	}
jah128	2:c81f4ef63132	453	break;
jah128	2:c81f4ef63132	454	}
jah128	2:c81f4ef63132	455
jah128	2:c81f4ef63132	456	case 13: // Request line-tracking IR reading
jah128	2:c81f4ef63132	457	break;
jah128	2:c81f4ef63132	458	case 14: // Request uptime
jah128	2:c81f4ef63132	459	break;
jah128	2:c81f4ef63132	460	case 15: //
jah128	2:c81f4ef63132	461	break;
jah128	2:c81f4ef63132	462	}
jah128	2:c81f4ef63132	463	send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
jah128	2:c81f4ef63132	464
jah128	2:c81f4ef63132	465	}
jah128	2:c81f4ef63132	466
jah128	2:c81f4ef63132	467
jah128	2:c81f4ef63132	468	void errormessage(int index)
jah128	2:c81f4ef63132	469	{
jah128	2:c81f4ef63132	470	if(RF_USE_LEDS==1) errorled=1;
jah128	2:c81f4ef63132	471	switch(index) {
jah128	2:c81f4ef63132	472	case 0: //Message to short
jah128	2:c81f4ef63132	473	if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
jah128	2:c81f4ef63132	474	break;
jah128	2:c81f4ef63132	475	case 1: //Sender out of valid range
jah128	2:c81f4ef63132	476	if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
jah128	2:c81f4ef63132	477	break;
jah128	2:c81f4ef63132	478	case 2: //Target out of valid range
jah128	2:c81f4ef63132	479	if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
jah128	2:c81f4ef63132	480	break;
jah128	2:c81f4ef63132	481	case 3:
jah128	2:c81f4ef63132	482
jah128	2:c81f4ef63132	483	break;
jah128	2:c81f4ef63132	484	case 4:
jah128	2:c81f4ef63132	485	break;
jah128	2:c81f4ef63132	486	}
jah128	2:c81f4ef63132	487	}
jah128	2:c81f4ef63132	488
jah128	2:c81f4ef63132	489
jah128	2:c81f4ef63132	490
jah128	2:c81f4ef63132	491
jah128	2:c81f4ef63132	492
jah128	2:c81f4ef63132	493	void send_rf_request_null ( char target )
jah128	2:c81f4ef63132	494	{
jah128	2:c81f4ef63132	495	char command = 0x80;
jah128	2:c81f4ef63132	496	char length = 0;
jah128	2:c81f4ef63132	497	char * data = NULL;
jah128	2:c81f4ef63132	498	if(target == 0) { //Request broadcast to all recipients
jah128	2:c81f4ef63132	499	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	500	swarm[i].status_rf_request_null = 1;
jah128	2:c81f4ef63132	501	}
jah128	2:c81f4ef63132	502	} else swarm[target].status_rf_request_null = 1;
jah128	2:c81f4ef63132	503	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	504	}
jah128	2:c81f4ef63132	505
jah128	2:c81f4ef63132	506
jah128	2:c81f4ef63132	507	void send_rf_request_left_motor_speed ( char target )
jah128	2:c81f4ef63132	508	{
jah128	2:c81f4ef63132	509	char command = 0x81;
jah128	2:c81f4ef63132	510	char length = 0;
jah128	2:c81f4ef63132	511	char * data = NULL;
jah128	2:c81f4ef63132	512	if(target == 0) {
jah128	2:c81f4ef63132	513	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	514	swarm[i].status_rf_request_left_motor_speed = 1;
jah128	2:c81f4ef63132	515	}
jah128	2:c81f4ef63132	516	} else swarm[target].status_rf_request_left_motor_speed = 1;
jah128	2:c81f4ef63132	517	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	518	}
jah128	2:c81f4ef63132	519
jah128	2:c81f4ef63132	520	void send_rf_request_right_motor_speed ( char target )
jah128	2:c81f4ef63132	521	{
jah128	2:c81f4ef63132	522	char command = 0x82;
jah128	2:c81f4ef63132	523	char length = 0;
jah128	2:c81f4ef63132	524	char * data = NULL;
jah128	2:c81f4ef63132	525	if(target == 0) {
jah128	2:c81f4ef63132	526	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	527	swarm[i].status_rf_request_right_motor_speed = 1;
jah128	2:c81f4ef63132	528	}
jah128	2:c81f4ef63132	529	} else swarm[target].status_rf_request_right_motor_speed = 1;
jah128	2:c81f4ef63132	530	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	531	}
jah128	2:c81f4ef63132	532
jah128	2:c81f4ef63132	533	void send_rf_request_button_state ( char target )
jah128	2:c81f4ef63132	534	{
jah128	2:c81f4ef63132	535	char command = 0x83;
jah128	2:c81f4ef63132	536	char length = 0;
jah128	2:c81f4ef63132	537	char * data = NULL;
jah128	2:c81f4ef63132	538	if(target == 0) {
jah128	2:c81f4ef63132	539	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	540	swarm[i].status_rf_request_button_state = 1;
jah128	2:c81f4ef63132	541	}
jah128	2:c81f4ef63132	542	} else swarm[target].status_rf_request_button_state = 1;
jah128	2:c81f4ef63132	543	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	544	}
jah128	2:c81f4ef63132	545
jah128	2:c81f4ef63132	546	void send_rf_request_led_colour ( char target )
jah128	2:c81f4ef63132	547	{
jah128	2:c81f4ef63132	548	char command = 0x84;
jah128	2:c81f4ef63132	549	char length = 0;
jah128	2:c81f4ef63132	550	char * data = NULL;
jah128	2:c81f4ef63132	551	if(target == 0) {
jah128	2:c81f4ef63132	552	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	553	swarm[i].status_rf_request_led_colour = 1;
jah128	2:c81f4ef63132	554	}
jah128	2:c81f4ef63132	555	} else swarm[target].status_rf_request_led_colour = 1;
jah128	2:c81f4ef63132	556	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	557	}
jah128	2:c81f4ef63132	558
jah128	2:c81f4ef63132	559	void send_rf_request_led_states ( char target )
jah128	2:c81f4ef63132	560	{
jah128	2:c81f4ef63132	561	char command = 0x85;
jah128	2:c81f4ef63132	562	char length = 0;
jah128	2:c81f4ef63132	563	char * data = NULL;
jah128	2:c81f4ef63132	564	if(target == 0) {
jah128	2:c81f4ef63132	565	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	566	swarm[i].status_rf_request_led_states = 1;
jah128	2:c81f4ef63132	567	}
jah128	2:c81f4ef63132	568	} else swarm[target].status_rf_request_led_states = 1;
jah128	2:c81f4ef63132	569	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	570	}
jah128	2:c81f4ef63132	571
jah128	2:c81f4ef63132	572	void send_rf_request_battery ( char target )
jah128	2:c81f4ef63132	573	{
jah128	2:c81f4ef63132	574	char command = 0x86;
jah128	2:c81f4ef63132	575	char length = 0;
jah128	2:c81f4ef63132	576	char * data = NULL;
jah128	2:c81f4ef63132	577	if(target == 0) {
jah128	2:c81f4ef63132	578	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	579	swarm[i].status_rf_request_battery = 1;
jah128	2:c81f4ef63132	580	}
jah128	2:c81f4ef63132	581	} else swarm[target].status_rf_request_battery = 1;
jah128	2:c81f4ef63132	582	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	583	}
jah128	2:c81f4ef63132	584
jah128	2:c81f4ef63132	585	void send_rf_request_light_sensor ( char target )
jah128	2:c81f4ef63132	586	{
jah128	2:c81f4ef63132	587	char command = 0x87;
jah128	2:c81f4ef63132	588	char length = 0;
jah128	2:c81f4ef63132	589	char * data = NULL;
jah128	2:c81f4ef63132	590	if(target == 0) {
jah128	2:c81f4ef63132	591	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	592	swarm[i].status_rf_request_light_sensor = 1;
jah128	2:c81f4ef63132	593	}
jah128	2:c81f4ef63132	594	} else swarm[target].status_rf_request_light_sensor = 1;
jah128	2:c81f4ef63132	595	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	596	}
jah128	2:c81f4ef63132	597
jah128	2:c81f4ef63132	598	void send_rf_request_accelerometer ( char target )
jah128	2:c81f4ef63132	599	{
jah128	2:c81f4ef63132	600	char command = 0x88;
jah128	2:c81f4ef63132	601	char length = 0;
jah128	2:c81f4ef63132	602	char * data = NULL;
jah128	2:c81f4ef63132	603	if(target == 0) {
jah128	2:c81f4ef63132	604	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	605	swarm[i].status_rf_request_accelerometer = 1;
jah128	2:c81f4ef63132	606	}
jah128	2:c81f4ef63132	607	} else swarm[target].status_rf_request_accelerometer = 1;
jah128	2:c81f4ef63132	608	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	609	}
jah128	2:c81f4ef63132	610
jah128	2:c81f4ef63132	611	void send_rf_request_gyroscope ( char target )
jah128	2:c81f4ef63132	612	{
jah128	2:c81f4ef63132	613	char command = 0x89;
jah128	2:c81f4ef63132	614	char length = 0;
jah128	2:c81f4ef63132	615	char * data = NULL;
jah128	2:c81f4ef63132	616	if(target == 0) {
jah128	2:c81f4ef63132	617	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	618	swarm[i].status_rf_request_gyroscope = 1;
jah128	2:c81f4ef63132	619	}
jah128	2:c81f4ef63132	620	} else swarm[target].status_rf_request_gyroscope = 1;
jah128	2:c81f4ef63132	621	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	622	}
jah128	2:c81f4ef63132	623
jah128	2:c81f4ef63132	624	void send_rf_request_background_ir ( char target )
jah128	2:c81f4ef63132	625	{
jah128	2:c81f4ef63132	626	char command = 0x8A;
jah128	2:c81f4ef63132	627	char length = 0;
jah128	2:c81f4ef63132	628	char * data = NULL;
jah128	2:c81f4ef63132	629	if(target == 0) {
jah128	2:c81f4ef63132	630	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	631	swarm[i].status_rf_request_background_ir = 1;
jah128	2:c81f4ef63132	632	}
jah128	2:c81f4ef63132	633	} else swarm[target].status_rf_request_background_ir = 1;
jah128	2:c81f4ef63132	634	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	635	}
jah128	2:c81f4ef63132	636
jah128	2:c81f4ef63132	637	void send_rf_request_reflected_ir ( char target )
jah128	2:c81f4ef63132	638	{
jah128	2:c81f4ef63132	639	char command = 0x8B;
jah128	2:c81f4ef63132	640	char length = 0;
jah128	2:c81f4ef63132	641	char * data = NULL;
jah128	2:c81f4ef63132	642	if(target == 0) {
jah128	2:c81f4ef63132	643	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	644	swarm[i].status_rf_request_reflected_ir = 1;
jah128	2:c81f4ef63132	645	}
jah128	2:c81f4ef63132	646	} else swarm[target].status_rf_request_reflected_ir = 1;
jah128	2:c81f4ef63132	647	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	648	}
jah128	2:c81f4ef63132	649
jah128	2:c81f4ef63132	650	void send_rf_request_distance_ir ( char target )
jah128	2:c81f4ef63132	651	{
jah128	2:c81f4ef63132	652	char command = 0x8C;
jah128	2:c81f4ef63132	653	char length = 0;
jah128	2:c81f4ef63132	654	char * data = NULL;
jah128	2:c81f4ef63132	655	if(target == 0) {
jah128	2:c81f4ef63132	656	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	657	swarm[i].status_rf_request_distance_ir = 1;
jah128	2:c81f4ef63132	658	}
jah128	2:c81f4ef63132	659	} else swarm[target].status_rf_request_distance_ir = 1;
jah128	2:c81f4ef63132	660	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	661	}
jah128	2:c81f4ef63132	662
jah128	2:c81f4ef63132	663	void send_rf_request_line_following_ir ( char target )
jah128	2:c81f4ef63132	664	{
jah128	2:c81f4ef63132	665	char command = 0x8D;
jah128	2:c81f4ef63132	666	char length = 0;
jah128	2:c81f4ef63132	667	char * data = NULL;
jah128	2:c81f4ef63132	668	if(target == 0) {
jah128	2:c81f4ef63132	669	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	670	swarm[i].status_rf_request_line_following_ir = 1;
jah128	2:c81f4ef63132	671	}
jah128	2:c81f4ef63132	672	} else swarm[target].status_rf_request_line_following_ir = 1;
jah128	2:c81f4ef63132	673	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	674	}
jah128	2:c81f4ef63132	675
jah128	2:c81f4ef63132	676	void send_rf_request_uptime ( char target )
jah128	2:c81f4ef63132	677	{
jah128	2:c81f4ef63132	678	char command = 0x8E;
jah128	2:c81f4ef63132	679	char length = 0;
jah128	2:c81f4ef63132	680	char * data = NULL;
jah128	2:c81f4ef63132	681	if(target == 0) {
jah128	2:c81f4ef63132	682	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	683	swarm[i].status_rf_request_uptime= 1;
jah128	2:c81f4ef63132	684	}
jah128	2:c81f4ef63132	685	} else swarm[target].status_rf_request_uptime = 1;
jah128	2:c81f4ef63132	686	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	687	}
jah128	2:c81f4ef63132	688
jah128	2:c81f4ef63132	689	void send_rf_command_stop ( char target, char request_response )
jah128	2:c81f4ef63132	690	{
jah128	2:c81f4ef63132	691	char command = 0x10;
jah128	2:c81f4ef63132	692	char length = 0;
jah128	2:c81f4ef63132	693	char data [0];
jah128	2:c81f4ef63132	694	if(request_response == 1) {
jah128	2:c81f4ef63132	695	command+=64;
jah128	2:c81f4ef63132	696	if(target == 0) {
jah128	2:c81f4ef63132	697	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	698	swarm[i].status_rf_command_stop= 1;
jah128	2:c81f4ef63132	699	}
jah128	2:c81f4ef63132	700	} else swarm[target].status_rf_command_stop = 1;
jah128	2:c81f4ef63132	701	}
jah128	2:c81f4ef63132	702	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	703	}
jah128	2:c81f4ef63132	704
jah128	2:c81f4ef63132	705	void send_rf_command_forward ( char target, char request_response, float speed )
jah128	2:c81f4ef63132	706	{
jah128	2:c81f4ef63132	707	char command = 0x11;
jah128	2:c81f4ef63132	708	char length = 2;
jah128	2:c81f4ef63132	709	char data [2];
jah128	2:c81f4ef63132	710	float qspeed = speed + 1;
jah128	2:c81f4ef63132	711	qspeed *= 32768.0;
jah128	2:c81f4ef63132	712	int ispeed = (int) qspeed;
jah128	2:c81f4ef63132	713	data[0] = (char) (ispeed >> 8);
jah128	2:c81f4ef63132	714	data[1] = (char) (ispeed % 256);
jah128	2:c81f4ef63132	715	if(request_response == 1) {
jah128	2:c81f4ef63132	716	command+=64;
jah128	2:c81f4ef63132	717	if(target == 0) {
jah128	2:c81f4ef63132	718	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	719	swarm[i].status_rf_command_forward= 1;
jah128	2:c81f4ef63132	720	}
jah128	2:c81f4ef63132	721	} else swarm[target].status_rf_command_forward = 1;
jah128	2:c81f4ef63132	722	}
jah128	2:c81f4ef63132	723	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	724	}
jah128	2:c81f4ef63132	725
jah128	2:c81f4ef63132	726
jah128	2:c81f4ef63132	727	void send_rf_command_backward ( char target, char request_response, float speed )
jah128	2:c81f4ef63132	728	{
jah128	2:c81f4ef63132	729	char command = 0x12;
jah128	2:c81f4ef63132	730	char length = 2;
jah128	2:c81f4ef63132	731	char data [2];
jah128	2:c81f4ef63132	732	float qspeed = speed + 1;
jah128	2:c81f4ef63132	733	qspeed *= 32768.0;
jah128	2:c81f4ef63132	734	int ispeed = (int) qspeed;
jah128	2:c81f4ef63132	735	data[0] = (char) (ispeed >> 8);
jah128	2:c81f4ef63132	736	data[1] = (char) (ispeed % 256);
jah128	2:c81f4ef63132	737	if(request_response == 1) {
jah128	2:c81f4ef63132	738	command+=64;
jah128	2:c81f4ef63132	739	if(target == 0) {
jah128	2:c81f4ef63132	740	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	741	swarm[i].status_rf_command_backward= 1;
jah128	2:c81f4ef63132	742	}
jah128	2:c81f4ef63132	743	} else swarm[target].status_rf_command_backward = 1;
jah128	2:c81f4ef63132	744	}
jah128	2:c81f4ef63132	745	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	746	}
jah128	2:c81f4ef63132	747
jah128	2:c81f4ef63132	748	void send_rf_command_left ( char target, char request_response, float speed )
jah128	2:c81f4ef63132	749	{
jah128	2:c81f4ef63132	750	char command = 0x13;
jah128	2:c81f4ef63132	751	char length = 2;
jah128	2:c81f4ef63132	752	char data [2];
jah128	2:c81f4ef63132	753	float qspeed = speed + 1;
jah128	2:c81f4ef63132	754	qspeed *= 32768.0;
jah128	2:c81f4ef63132	755	int ispeed = (int) qspeed;
jah128	2:c81f4ef63132	756	data[0] = (char) (ispeed >> 8);
jah128	2:c81f4ef63132	757	data[1] = (char) (ispeed % 256);
jah128	2:c81f4ef63132	758	if(request_response == 1) {
jah128	2:c81f4ef63132	759	command+=64;
jah128	2:c81f4ef63132	760	if(target == 0) {
jah128	2:c81f4ef63132	761	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	762	swarm[i].status_rf_command_left = 1;
jah128	2:c81f4ef63132	763	}
jah128	2:c81f4ef63132	764	} else swarm[target].status_rf_command_left = 1;
jah128	2:c81f4ef63132	765	}
jah128	2:c81f4ef63132	766	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	767	}
jah128	2:c81f4ef63132	768
jah128	2:c81f4ef63132	769	void send_rf_command_right ( char target, char request_response, float speed )
jah128	2:c81f4ef63132	770	{
jah128	2:c81f4ef63132	771	char command = 0x14;
jah128	2:c81f4ef63132	772	char length = 2;
jah128	2:c81f4ef63132	773	char data [2];
jah128	2:c81f4ef63132	774	float qspeed = speed + 1;
jah128	2:c81f4ef63132	775	qspeed *= 32768.0;
jah128	2:c81f4ef63132	776	int ispeed = (int) qspeed;
jah128	2:c81f4ef63132	777	data[0] = (char) (ispeed >> 8);
jah128	2:c81f4ef63132	778	data[1] = (char) (ispeed % 256);
jah128	2:c81f4ef63132	779	if(request_response == 1) {
jah128	2:c81f4ef63132	780	command+=64;
jah128	2:c81f4ef63132	781	if(target == 0) {
jah128	2:c81f4ef63132	782	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	783	swarm[i].status_rf_command_right = 1;
jah128	2:c81f4ef63132	784	}
jah128	2:c81f4ef63132	785	} else swarm[target].status_rf_command_right = 1;
jah128	2:c81f4ef63132	786	}
jah128	2:c81f4ef63132	787	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	788	}
jah128	2:c81f4ef63132	789
jah128	2:c81f4ef63132	790	void send_rf_command_left_motor ( char target, char request_response, float speed )
jah128	2:c81f4ef63132	791	{
jah128	2:c81f4ef63132	792	char command = 0x15;
jah128	2:c81f4ef63132	793	char length = 2;
jah128	2:c81f4ef63132	794	char data [2];
jah128	2:c81f4ef63132	795	float qspeed = speed + 1;
jah128	2:c81f4ef63132	796	qspeed *= 32768.0;
jah128	2:c81f4ef63132	797	int ispeed = (int) qspeed;
jah128	2:c81f4ef63132	798	data[0] = (char) (ispeed >> 8);
jah128	2:c81f4ef63132	799	data[1] = (char) (ispeed % 256);
jah128	2:c81f4ef63132	800	if(request_response == 1) {
jah128	2:c81f4ef63132	801	command+=64;
jah128	2:c81f4ef63132	802	if(target == 0) {
jah128	2:c81f4ef63132	803	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	804	swarm[i].status_rf_command_left_motor = 1;
jah128	2:c81f4ef63132	805	}
jah128	2:c81f4ef63132	806	} else swarm[target].status_rf_command_left_motor = 1;
jah128	2:c81f4ef63132	807	}
jah128	2:c81f4ef63132	808	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	809	}
jah128	2:c81f4ef63132	810
jah128	2:c81f4ef63132	811	void send_rf_command_right_motor ( char target, char request_response, float speed )
jah128	2:c81f4ef63132	812	{
jah128	2:c81f4ef63132	813	char command = 0x16;
jah128	2:c81f4ef63132	814	char length = 2;
jah128	2:c81f4ef63132	815	char data [2];
jah128	2:c81f4ef63132	816	float qspeed = speed + 1;
jah128	2:c81f4ef63132	817	qspeed *= 32768.0;
jah128	2:c81f4ef63132	818	int ispeed = (int) qspeed;
jah128	2:c81f4ef63132	819	data[0] = (char) (ispeed >> 8);
jah128	2:c81f4ef63132	820	data[1] = (char) (ispeed % 256);
jah128	2:c81f4ef63132	821	if(request_response == 1) {
jah128	2:c81f4ef63132	822	command+=64;
jah128	2:c81f4ef63132	823	if(target == 0) {
jah128	2:c81f4ef63132	824	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	825	swarm[i].status_rf_command_right_motor = 1;
jah128	2:c81f4ef63132	826	}
jah128	2:c81f4ef63132	827	} else swarm[target].status_rf_command_right_motor = 1;
jah128	2:c81f4ef63132	828	}
jah128	2:c81f4ef63132	829	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	830	}
jah128	2:c81f4ef63132	831
jah128	2:c81f4ef63132	832	void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue )
jah128	2:c81f4ef63132	833	{
jah128	2:c81f4ef63132	834	char command = 0x17;
jah128	2:c81f4ef63132	835	char length = 3;
jah128	2:c81f4ef63132	836	char data [3];
jah128	2:c81f4ef63132	837	data[0] = red;
jah128	2:c81f4ef63132	838	data[1] = green;
jah128	2:c81f4ef63132	839	data[2] = blue;
jah128	2:c81f4ef63132	840	if(request_response == 1) {
jah128	2:c81f4ef63132	841	command+=64;
jah128	2:c81f4ef63132	842	if(target == 0) {
jah128	2:c81f4ef63132	843	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	844	swarm[i].status_rf_command_oled_colour = 1;
jah128	2:c81f4ef63132	845	}
jah128	2:c81f4ef63132	846	} else swarm[target].status_rf_command_oled_colour = 1;
jah128	2:c81f4ef63132	847	}
jah128	2:c81f4ef63132	848	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	849	}
jah128	2:c81f4ef63132	850
jah128	2:c81f4ef63132	851	void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue )
jah128	2:c81f4ef63132	852	{
jah128	2:c81f4ef63132	853	char command = 0x18;
jah128	2:c81f4ef63132	854	char length = 3;
jah128	2:c81f4ef63132	855	char data [3];
jah128	2:c81f4ef63132	856	data[0] = red;
jah128	2:c81f4ef63132	857	data[1] = green;
jah128	2:c81f4ef63132	858	data[2] = blue;
jah128	2:c81f4ef63132	859	if(request_response == 1) {
jah128	2:c81f4ef63132	860	command+=64;
jah128	2:c81f4ef63132	861	if(target == 0) {
jah128	2:c81f4ef63132	862	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	863	swarm[i].status_rf_command_cled_colour = 1;
jah128	2:c81f4ef63132	864	}
jah128	2:c81f4ef63132	865	} else swarm[target].status_rf_command_cled_colour = 1;
jah128	2:c81f4ef63132	866	}
jah128	2:c81f4ef63132	867	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	868	}
jah128	2:c81f4ef63132	869
jah128	2:c81f4ef63132	870	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 )
jah128	2:c81f4ef63132	871	{
jah128	2:c81f4ef63132	872	char command = 0x19;
jah128	2:c81f4ef63132	873	char length = 2;
jah128	2:c81f4ef63132	874	char data [2];
jah128	2:c81f4ef63132	875	data[0] = 0;
jah128	2:c81f4ef63132	876	data[1] = 0;
jah128	2:c81f4ef63132	877	if( led0 == 1) data[0] += 2;
jah128	2:c81f4ef63132	878	if( led1 == 1) data[0] += 1;
jah128	2:c81f4ef63132	879	if( led2 == 1) data[1] += 128;
jah128	2:c81f4ef63132	880	if( led3 == 1) data[1] += 64;
jah128	2:c81f4ef63132	881	if( led4 == 1) data[1] += 32;
jah128	2:c81f4ef63132	882	if( led5 == 1) data[1] += 16;
jah128	2:c81f4ef63132	883	if( led6 == 1) data[1] += 8;
jah128	2:c81f4ef63132	884	if( led7 == 1) data[1] += 4;
jah128	2:c81f4ef63132	885	if( led8 == 1) data[1] += 2;
jah128	2:c81f4ef63132	886	if( led9 == 1) data[1] += 1;
jah128	2:c81f4ef63132	887	if(request_response == 1) {
jah128	2:c81f4ef63132	888	command+=64;
jah128	2:c81f4ef63132	889	if(target == 0) {
jah128	2:c81f4ef63132	890	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	891	swarm[i].status_rf_command_oled_state = 1;
jah128	2:c81f4ef63132	892	}
jah128	2:c81f4ef63132	893	} else swarm[target].status_rf_command_oled_state = 1;
jah128	2:c81f4ef63132	894	}
jah128	2:c81f4ef63132	895	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	896	}
jah128	2:c81f4ef63132	897
jah128	2:c81f4ef63132	898	void send_rf_command_cled_state ( char target, char request_response, char enable )
jah128	2:c81f4ef63132	899	{
jah128	2:c81f4ef63132	900	char command = 0x1A;
jah128	2:c81f4ef63132	901	char length = 1;
jah128	2:c81f4ef63132	902	char data [1];
jah128	2:c81f4ef63132	903	data[0] = enable;
jah128	2:c81f4ef63132	904	if(request_response == 1) {
jah128	2:c81f4ef63132	905	command+=64;
jah128	2:c81f4ef63132	906	if(target == 0) {
jah128	2:c81f4ef63132	907	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	908	swarm[i].status_rf_command_cled_state = 1;
jah128	2:c81f4ef63132	909	}
jah128	2:c81f4ef63132	910	} else swarm[target].status_rf_command_cled_state = 1;
jah128	2:c81f4ef63132	911	}
jah128	2:c81f4ef63132	912	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	913	}
jah128	2:c81f4ef63132	914
jah128	2:c81f4ef63132	915	void send_rf_command_set_oled ( char target, char request_response, char oled, char enable )
jah128	2:c81f4ef63132	916	{
jah128	2:c81f4ef63132	917	char command = 0x1B;
jah128	2:c81f4ef63132	918	char length = 1;
jah128	2:c81f4ef63132	919	char data [1];
jah128	2:c81f4ef63132	920	data[0] = oled;
jah128	2:c81f4ef63132	921	if(enable == 1) oled+= 32;
jah128	2:c81f4ef63132	922	if(request_response == 1) {
jah128	2:c81f4ef63132	923	command+=64;
jah128	2:c81f4ef63132	924	if(target == 0) {
jah128	2:c81f4ef63132	925	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	926	swarm[i].status_rf_command_set_oled = 1;
jah128	2:c81f4ef63132	927	}
jah128	2:c81f4ef63132	928	} else swarm[target].status_rf_command_set_oled = 1;
jah128	2:c81f4ef63132	929	}
jah128	2:c81f4ef63132	930	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	931	}
jah128	2:c81f4ef63132	932
jah128	2:c81f4ef63132	933	void send_rf_command_play_tune ( char target, char request_response, char * data, char length )
jah128	2:c81f4ef63132	934	{
jah128	2:c81f4ef63132	935	char command = 0x1C;
jah128	2:c81f4ef63132	936	if(request_response == 1) {
jah128	2:c81f4ef63132	937	command+=64;
jah128	2:c81f4ef63132	938	if(target == 0) {
jah128	2:c81f4ef63132	939	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	940	swarm[i].status_rf_command_play_tune = 1;
jah128	2:c81f4ef63132	941	}
jah128	2:c81f4ef63132	942	} else swarm[target].status_rf_command_play_tune = 1;
jah128	2:c81f4ef63132	943	}
jah128	2:c81f4ef63132	944	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	945	}
jah128	2:c81f4ef63132	946
jah128	2:c81f4ef63132	947	void send_rf_command_sync_time ( char target, char request_response )
jah128	2:c81f4ef63132	948	{
jah128	2:c81f4ef63132	949	char command = 0x1D;
jah128	2:c81f4ef63132	950	char length = 1;
jah128	2:c81f4ef63132	951	char data [1];
jah128	2:c81f4ef63132	952	data[0] = 0;
jah128	2:c81f4ef63132	953	if(request_response == 1) {
jah128	2:c81f4ef63132	954	command+=64;
jah128	2:c81f4ef63132	955	if(target == 0) {
jah128	2:c81f4ef63132	956	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	957	swarm[i].status_rf_command_sync_time = 1;
jah128	2:c81f4ef63132	958	}
jah128	2:c81f4ef63132	959	} else swarm[target].status_rf_command_sync_time = 1;
jah128	2:c81f4ef63132	960	}
jah128	2:c81f4ef63132	961	send_rf_message(target,command,data,length);
jah128	2:c81f4ef63132	962	}
jah128	2:c81f4ef63132	963
jah128	2:c81f4ef63132	964	//Resets the recorded swarm data tables
jah128	2:c81f4ef63132	965	void setup_communications()
jah128	2:c81f4ef63132	966	{
jah128	2:c81f4ef63132	967	for(int i=0; i<SWARM_SIZE; i++) {
jah128	2:c81f4ef63132	968	swarm[i].status_rf_request_null = 0;
jah128	2:c81f4ef63132	969	swarm[i].status_rf_request_left_motor_speed = 0;
jah128	2:c81f4ef63132	970	swarm[i].status_rf_request_right_motor_speed = 0;
jah128	2:c81f4ef63132	971	swarm[i].status_rf_request_button_state = 0;
jah128	2:c81f4ef63132	972	swarm[i].status_rf_request_led_colour = 0;
jah128	2:c81f4ef63132	973	swarm[i].status_rf_request_led_states = 0;
jah128	2:c81f4ef63132	974	swarm[i].status_rf_request_battery = 0;
jah128	2:c81f4ef63132	975	swarm[i].status_rf_request_light_sensor = 0;
jah128	2:c81f4ef63132	976	swarm[i].status_rf_request_accelerometer = 0;
jah128	2:c81f4ef63132	977	swarm[i].status_rf_request_gyroscope = 0;
jah128	2:c81f4ef63132	978	swarm[i].status_rf_request_background_ir = 0;
jah128	2:c81f4ef63132	979	swarm[i].status_rf_request_reflected_ir = 0;
jah128	2:c81f4ef63132	980	swarm[i].status_rf_request_distance_ir = 0;
jah128	2:c81f4ef63132	981	swarm[i].status_rf_request_line_following_ir = 0;
jah128	2:c81f4ef63132	982	swarm[i].status_rf_request_uptime = 0;
jah128	2:c81f4ef63132	983	swarm[i].status_rf_command_stop = 0;
jah128	2:c81f4ef63132	984	swarm[i].status_rf_command_forward = 0;
jah128	2:c81f4ef63132	985	swarm[i].status_rf_command_backward = 0;
jah128	2:c81f4ef63132	986	swarm[i].status_rf_command_left = 0;
jah128	2:c81f4ef63132	987	swarm[i].status_rf_command_right = 0;
jah128	2:c81f4ef63132	988	swarm[i].status_rf_command_left_motor = 0;
jah128	2:c81f4ef63132	989	swarm[i].status_rf_command_right_motor = 0;
jah128	2:c81f4ef63132	990	swarm[i].status_rf_command_oled_colour = 0;
jah128	2:c81f4ef63132	991	swarm[i].status_rf_command_cled_colour = 0;
jah128	2:c81f4ef63132	992	swarm[i].status_rf_command_oled_state = 0;
jah128	2:c81f4ef63132	993	swarm[i].status_rf_command_cled_state = 0;
jah128	2:c81f4ef63132	994	swarm[i].status_rf_command_set_oled = 0;
jah128	2:c81f4ef63132	995	swarm[i].status_rf_command_play_tune = 0;
jah128	2:c81f4ef63132	996	swarm[i].status_rf_command_sync_time = 0;
jah128	2:c81f4ef63132	997	swarm[i].left_motor_speed = 0.0;
jah128	2:c81f4ef63132	998	swarm[i].right_motor_speed = 0.0;
jah128	2:c81f4ef63132	999	swarm[i].button_state = 0;
jah128	2:c81f4ef63132	1000	for(int k=0; k<3; k++) {
jah128	2:c81f4ef63132	1001	swarm[i].outer_led_colour [k]=0;
jah128	2:c81f4ef63132	1002	swarm[i].center_led_colour [k]=0;
jah128	2:c81f4ef63132	1003	swarm[i].accelerometer [k]=0;
jah128	2:c81f4ef63132	1004	}
jah128	2:c81f4ef63132	1005	swarm[i].led_states[0]=0;
jah128	2:c81f4ef63132	1006	swarm[i].led_states[1]=0;
jah128	2:c81f4ef63132	1007	swarm[i].battery = 0.0;
jah128	2:c81f4ef63132	1008	swarm[i].light_sensor = 0.0;
jah128	2:c81f4ef63132	1009	swarm[i].gyro = 0.0;
jah128	2:c81f4ef63132	1010	for(int k=0; k<8; k++) {
jah128	2:c81f4ef63132	1011	swarm[i].background_ir[k] = 0;
jah128	2:c81f4ef63132	1012	swarm[i].reflected_ir[k] = 0;
jah128	2:c81f4ef63132	1013	swarm[i].distance_ir[k] = 0;
jah128	2:c81f4ef63132	1014	}
jah128	2:c81f4ef63132	1015	}
jah128	2:c81f4ef63132	1016	}
jah128	2:c81f4ef63132	1017
jah128	2:c81f4ef63132	1018
jah128	2:c81f4ef63132	1019	// Handle a message that is a response to a predefined (non-user) command or request
jah128	2:c81f4ef63132	1020	void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
jah128	2:c81f4ef63132	1021	{
jah128	2:c81f4ef63132	1022	char outcome = 0;
jah128	2:c81f4ef63132	1023	if(is_command == 0) {
jah128	2:c81f4ef63132	1024	// Response is a data_request_response
jah128	2:c81f4ef63132	1025	switch(function) {
jah128	2:c81f4ef63132	1026	case 0: {
jah128	2:c81f4ef63132	1027	if(swarm[sender].status_rf_request_null == 1) {
jah128	2:c81f4ef63132	1028	if(success == 1){
jah128	2:c81f4ef63132	1029	if(length == 0) {
jah128	2:c81f4ef63132	1030	swarm[sender].status_rf_request_null = 2;
jah128	2:c81f4ef63132	1031	outcome = 1;
jah128	2:c81f4ef63132	1032	} else {
jah128	2:c81f4ef63132	1033	swarm[sender].status_rf_request_null= 3;
jah128	2:c81f4ef63132	1034	outcome = 3;
jah128	2:c81f4ef63132	1035	}
jah128	2:c81f4ef63132	1036	} else {
jah128	2:c81f4ef63132	1037	swarm[sender].status_rf_request_null = 4;
jah128	2:c81f4ef63132	1038	outcome = 4;
jah128	2:c81f4ef63132	1039	}
jah128	2:c81f4ef63132	1040	} else outcome = 2;
jah128	2:c81f4ef63132	1041	}
jah128	2:c81f4ef63132	1042	break;
jah128	2:c81f4ef63132	1043
jah128	2:c81f4ef63132	1044	case 1: { //Left motor speed
jah128	2:c81f4ef63132	1045	if(swarm[sender].status_rf_request_left_motor_speed == 1) {
jah128	2:c81f4ef63132	1046	if(success == 1){
jah128	2:c81f4ef63132	1047	if(length == 2) {
jah128	2:c81f4ef63132	1048	swarm[sender].status_rf_request_left_motor_speed = 2;
jah128	2:c81f4ef63132	1049	int value = (data [0] << 8) + data[1];
jah128	2:c81f4ef63132	1050	value -= 32768;
jah128	2:c81f4ef63132	1051	float val = value;
jah128	2:c81f4ef63132	1052	val /= 32767.0;
jah128	2:c81f4ef63132	1053	swarm[sender].left_motor_speed = val;
jah128	2:c81f4ef63132	1054	outcome = 1;
jah128	2:c81f4ef63132	1055	} else {
jah128	2:c81f4ef63132	1056	swarm[sender].status_rf_request_left_motor_speed= 3;
jah128	2:c81f4ef63132	1057	outcome = 3;
jah128	2:c81f4ef63132	1058	}
jah128	2:c81f4ef63132	1059	} else {
jah128	2:c81f4ef63132	1060	swarm[sender].status_rf_request_left_motor_speed = 4;
jah128	2:c81f4ef63132	1061	outcome = 4;
jah128	2:c81f4ef63132	1062	}
jah128	2:c81f4ef63132	1063	} else outcome = 2;
jah128	2:c81f4ef63132	1064	}
jah128	2:c81f4ef63132	1065	break;
jah128	2:c81f4ef63132	1066
jah128	2:c81f4ef63132	1067	case 2: { //Right motor speed
jah128	2:c81f4ef63132	1068	if(swarm[sender].status_rf_request_right_motor_speed == 1) {
jah128	2:c81f4ef63132	1069	if(success == 1){
jah128	2:c81f4ef63132	1070	if(length == 2) {
jah128	2:c81f4ef63132	1071	swarm[sender].status_rf_request_right_motor_speed = 2;
jah128	2:c81f4ef63132	1072	int value = (data [0] << 8) + data[1];
jah128	2:c81f4ef63132	1073	value -= 32768;
jah128	2:c81f4ef63132	1074	float val = value;
jah128	2:c81f4ef63132	1075	val /= 32767.0;
jah128	2:c81f4ef63132	1076	swarm[sender].right_motor_speed = val;
jah128	2:c81f4ef63132	1077	outcome = 1;
jah128	2:c81f4ef63132	1078	} else {
jah128	2:c81f4ef63132	1079	swarm[sender].status_rf_request_right_motor_speed = 3;
jah128	2:c81f4ef63132	1080	outcome = 3;
jah128	2:c81f4ef63132	1081	}
jah128	2:c81f4ef63132	1082	} else {
jah128	2:c81f4ef63132	1083	swarm[sender].status_rf_request_right_motor_speed = 4;
jah128	2:c81f4ef63132	1084	outcome = 4;
jah128	2:c81f4ef63132	1085	}
jah128	2:c81f4ef63132	1086	} else outcome = 2;
jah128	2:c81f4ef63132	1087	}
jah128	2:c81f4ef63132	1088	break;
jah128	2:c81f4ef63132	1089
jah128	2:c81f4ef63132	1090	case 3: { //Button state
jah128	2:c81f4ef63132	1091	if(swarm[sender].status_rf_request_button_state == 1) {
jah128	2:c81f4ef63132	1092	if(success == 1){
jah128	2:c81f4ef63132	1093	if(length == 2) {
jah128	2:c81f4ef63132	1094	swarm[sender].status_rf_request_button_state = 2;
jah128	2:c81f4ef63132	1095	swarm[sender].button_state = data[0];
jah128	2:c81f4ef63132	1096	outcome = 1;
jah128	2:c81f4ef63132	1097	} else {
jah128	2:c81f4ef63132	1098	swarm[sender].status_rf_request_button_state = 3;
jah128	2:c81f4ef63132	1099	outcome = 3;
jah128	2:c81f4ef63132	1100	}
jah128	2:c81f4ef63132	1101	} else {
jah128	2:c81f4ef63132	1102	swarm[sender].status_rf_request_button_state = 4;
jah128	2:c81f4ef63132	1103	outcome = 4;
jah128	2:c81f4ef63132	1104	}
jah128	2:c81f4ef63132	1105	} else outcome = 2;
jah128	2:c81f4ef63132	1106	}
jah128	2:c81f4ef63132	1107	break;
jah128	2:c81f4ef63132	1108
jah128	2:c81f4ef63132	1109	case 4: { //LED Colour
jah128	2:c81f4ef63132	1110	if(swarm[sender].status_rf_request_led_colour == 1) {
jah128	2:c81f4ef63132	1111	if(success == 1) {
jah128	2:c81f4ef63132	1112	if(length == 6) {
jah128	2:c81f4ef63132	1113	swarm[sender].status_rf_request_led_colour = 2;
jah128	2:c81f4ef63132	1114	for(int i=0; i<3; i++) {
jah128	2:c81f4ef63132	1115	swarm[sender].outer_led_colour[i] = data[i];
jah128	2:c81f4ef63132	1116	swarm[sender].center_led_colour[i] = data[i+3];
jah128	2:c81f4ef63132	1117	}
jah128	2:c81f4ef63132	1118	outcome = 1;
jah128	2:c81f4ef63132	1119	} else {
jah128	2:c81f4ef63132	1120	swarm[sender].status_rf_request_led_colour = 3;
jah128	2:c81f4ef63132	1121	outcome = 3;
jah128	2:c81f4ef63132	1122	}
jah128	2:c81f4ef63132	1123	} else {
jah128	2:c81f4ef63132	1124	swarm[sender].status_rf_request_led_colour = 4;
jah128	2:c81f4ef63132	1125	outcome = 4;
jah128	2:c81f4ef63132	1126	}
jah128	2:c81f4ef63132	1127	} else outcome = 2;
jah128	2:c81f4ef63132	1128	}
jah128	2:c81f4ef63132	1129	break;
jah128	2:c81f4ef63132	1130
jah128	2:c81f4ef63132	1131	case 5: { //LED States
jah128	2:c81f4ef63132	1132	if(swarm[sender].status_rf_request_led_states == 1) {
jah128	2:c81f4ef63132	1133	if(success == 1) {
jah128	2:c81f4ef63132	1134	if(length == 2) {
jah128	2:c81f4ef63132	1135	swarm[sender].status_rf_request_led_states = 2;
jah128	2:c81f4ef63132	1136	for(int i=0; i<3; i++) {
jah128	2:c81f4ef63132	1137	swarm[sender].led_states[0] = data[0];
jah128	2:c81f4ef63132	1138	swarm[sender].led_states[1] = data[1];
jah128	2:c81f4ef63132	1139	}
jah128	2:c81f4ef63132	1140	outcome = 1;
jah128	2:c81f4ef63132	1141	} else {
jah128	2:c81f4ef63132	1142	swarm[sender].status_rf_request_led_states = 3;
jah128	2:c81f4ef63132	1143	outcome = 3;
jah128	2:c81f4ef63132	1144	}
jah128	2:c81f4ef63132	1145	} else {
jah128	2:c81f4ef63132	1146	swarm[sender].status_rf_request_led_states = 4;
jah128	2:c81f4ef63132	1147	outcome = 4;
jah128	2:c81f4ef63132	1148	}
jah128	2:c81f4ef63132	1149	} else outcome = 2;
jah128	2:c81f4ef63132	1150	}
jah128	2:c81f4ef63132	1151	break;
jah128	2:c81f4ef63132	1152
jah128	2:c81f4ef63132	1153
jah128	2:c81f4ef63132	1154	case 6: { //Battery
jah128	2:c81f4ef63132	1155	if(swarm[sender].status_rf_request_battery == 1) {
jah128	2:c81f4ef63132	1156	if(success == 1) {
jah128	2:c81f4ef63132	1157	if(length == 2) {
jah128	2:c81f4ef63132	1158	swarm[sender].status_rf_request_battery = 2;
jah128	2:c81f4ef63132	1159	int fbattery = data[0] * 256;
jah128	2:c81f4ef63132	1160	fbattery += data[1];
jah128	2:c81f4ef63132	1161	swarm[sender].battery = (float) fbattery / 1000.0;
jah128	2:c81f4ef63132	1162	outcome = 1;
jah128	2:c81f4ef63132	1163	} else {
jah128	2:c81f4ef63132	1164	swarm[sender].status_rf_request_battery = 3;
jah128	2:c81f4ef63132	1165	outcome = 3;
jah128	2:c81f4ef63132	1166	}
jah128	2:c81f4ef63132	1167	} else {
jah128	2:c81f4ef63132	1168	swarm[sender].status_rf_request_battery = 4;
jah128	2:c81f4ef63132	1169	outcome = 4;
jah128	2:c81f4ef63132	1170	}
jah128	2:c81f4ef63132	1171	} else outcome = 2;
jah128	2:c81f4ef63132	1172	}
jah128	2:c81f4ef63132	1173	break;
jah128	2:c81f4ef63132	1174
jah128	2:c81f4ef63132	1175	case 7: { //Light sensor
jah128	2:c81f4ef63132	1176	if(swarm[sender].status_rf_request_light_sensor == 1) {
jah128	2:c81f4ef63132	1177	if(success == 1) {
jah128	2:c81f4ef63132	1178	if(length == 2) {
jah128	2:c81f4ef63132	1179	swarm[sender].status_rf_request_light_sensor = 2;
jah128	2:c81f4ef63132	1180	int ilight = data[0] * 256;
jah128	2:c81f4ef63132	1181	ilight += data[1];
jah128	2:c81f4ef63132	1182	float flight = (float) (ilight) / 655.0;
jah128	2:c81f4ef63132	1183	swarm[sender].light_sensor = flight;
jah128	2:c81f4ef63132	1184	outcome = 1;
jah128	2:c81f4ef63132	1185	} else {
jah128	2:c81f4ef63132	1186	swarm[sender].status_rf_request_light_sensor = 3;
jah128	2:c81f4ef63132	1187	outcome = 3;
jah128	2:c81f4ef63132	1188	}
jah128	2:c81f4ef63132	1189	} else {
jah128	2:c81f4ef63132	1190	swarm[sender].status_rf_request_light_sensor = 4;
jah128	2:c81f4ef63132	1191	outcome = 4;
jah128	2:c81f4ef63132	1192	}
jah128	2:c81f4ef63132	1193	} else outcome = 2;
jah128	2:c81f4ef63132	1194	}
jah128	2:c81f4ef63132	1195	break;
jah128	2:c81f4ef63132	1196
jah128	2:c81f4ef63132	1197	case 8: { //Accelerometer
jah128	2:c81f4ef63132	1198	if(swarm[sender].status_rf_request_accelerometer == 1) {
jah128	2:c81f4ef63132	1199	if(success == 1) {
jah128	2:c81f4ef63132	1200	if(length == 6) {
jah128	2:c81f4ef63132	1201	swarm[sender].status_rf_request_accelerometer = 2;
jah128	2:c81f4ef63132	1202	int acc_x = (data[0] * 256) + data[1];
jah128	2:c81f4ef63132	1203	int acc_y = (data[2] * 256) + data[3];
jah128	2:c81f4ef63132	1204	int acc_z = (data[4] * 256) + data[5];
jah128	2:c81f4ef63132	1205	swarm[sender].accelerometer[0] = (float) acc_x - 32768;
jah128	2:c81f4ef63132	1206	swarm[sender].accelerometer[1] = (float) acc_y - 32768;
jah128	2:c81f4ef63132	1207	swarm[sender].accelerometer[2] = (float) acc_z - 32768;
jah128	2:c81f4ef63132	1208	outcome = 1;
jah128	2:c81f4ef63132	1209	} else {
jah128	2:c81f4ef63132	1210	swarm[sender].status_rf_request_accelerometer = 3;
jah128	2:c81f4ef63132	1211	outcome = 3;
jah128	2:c81f4ef63132	1212	}
jah128	2:c81f4ef63132	1213	} else {
jah128	2:c81f4ef63132	1214	swarm[sender].status_rf_request_accelerometer = 4;
jah128	2:c81f4ef63132	1215	outcome = 4;
jah128	2:c81f4ef63132	1216	}
jah128	2:c81f4ef63132	1217	} else outcome = 2;
jah128	2:c81f4ef63132	1218	}
jah128	2:c81f4ef63132	1219	break;
jah128	2:c81f4ef63132	1220
jah128	2:c81f4ef63132	1221	case 9: { //Gyroscope
jah128	2:c81f4ef63132	1222	if(swarm[sender].status_rf_request_gyroscope == 1) {
jah128	2:c81f4ef63132	1223	if(success == 1) {
jah128	2:c81f4ef63132	1224	if(length == 2) {
jah128	2:c81f4ef63132	1225	swarm[sender].status_rf_request_gyroscope = 2;
jah128	2:c81f4ef63132	1226	int gyro = (data [0] * 256) + data[1];
jah128	2:c81f4ef63132	1227	swarm[sender].gyro = (float) gyro - 32768;
jah128	2:c81f4ef63132	1228	outcome = 1;
jah128	2:c81f4ef63132	1229	} else {
jah128	2:c81f4ef63132	1230	swarm[sender].status_rf_request_gyroscope = 3;
jah128	2:c81f4ef63132	1231	outcome = 3;
jah128	2:c81f4ef63132	1232	}
jah128	2:c81f4ef63132	1233	} else {
jah128	2:c81f4ef63132	1234	swarm[sender].status_rf_request_gyroscope = 4;
jah128	2:c81f4ef63132	1235	outcome = 4;
jah128	2:c81f4ef63132	1236	}
jah128	2:c81f4ef63132	1237	} else outcome = 2;
jah128	2:c81f4ef63132	1238	}
jah128	2:c81f4ef63132	1239	break;
jah128	2:c81f4ef63132	1240
jah128	2:c81f4ef63132	1241	case 10: { //Background IR
jah128	2:c81f4ef63132	1242	if(swarm[sender].status_rf_request_background_ir == 1) {
jah128	2:c81f4ef63132	1243	if(success == 1) {
jah128	2:c81f4ef63132	1244	if(length == 16) {
jah128	2:c81f4ef63132	1245	swarm[sender].status_rf_request_background_ir = 2;
jah128	2:c81f4ef63132	1246	for(int i=0;i<8;i++){
jah128	2:c81f4ef63132	1247	int offset = i * 2;
jah128	2:c81f4ef63132	1248	unsigned short value = (unsigned short) data[offset] << 8;
jah128	2:c81f4ef63132	1249	value += data[offset+1];
jah128	2:c81f4ef63132	1250	swarm[sender].background_ir[i]=value;
jah128	2:c81f4ef63132	1251	}
jah128	2:c81f4ef63132	1252	outcome = 1;
jah128	2:c81f4ef63132	1253	} else {
jah128	2:c81f4ef63132	1254	swarm[sender].status_rf_request_background_ir = 3;
jah128	2:c81f4ef63132	1255	outcome = 3;
jah128	2:c81f4ef63132	1256	}
jah128	2:c81f4ef63132	1257	} else {
jah128	2:c81f4ef63132	1258	swarm[sender].status_rf_request_background_ir = 4;
jah128	2:c81f4ef63132	1259	outcome = 4;
jah128	2:c81f4ef63132	1260	}
jah128	2:c81f4ef63132	1261	} else outcome = 2;
jah128	2:c81f4ef63132	1262	}
jah128	2:c81f4ef63132	1263	break;
jah128	2:c81f4ef63132	1264
jah128	2:c81f4ef63132	1265	case 11: { //Reflected IR
jah128	2:c81f4ef63132	1266	if(swarm[sender].status_rf_request_reflected_ir == 1) {
jah128	2:c81f4ef63132	1267	if(success == 1) {
jah128	2:c81f4ef63132	1268	if(length == 16) {
jah128	2:c81f4ef63132	1269	swarm[sender].status_rf_request_reflected_ir = 2;
jah128	2:c81f4ef63132	1270	for(int i=0;i<8;i++){
jah128	2:c81f4ef63132	1271	int offset = i * 2;
jah128	2:c81f4ef63132	1272	unsigned short value = (unsigned short) data[offset] << 8;
jah128	2:c81f4ef63132	1273	value += data[offset+1];
jah128	2:c81f4ef63132	1274	swarm[sender].reflected_ir[i]=value;
jah128	2:c81f4ef63132	1275	}
jah128	2:c81f4ef63132	1276	outcome = 1;
jah128	2:c81f4ef63132	1277	} else {
jah128	2:c81f4ef63132	1278	swarm[sender].status_rf_request_reflected_ir = 3;
jah128	2:c81f4ef63132	1279	outcome = 3;
jah128	2:c81f4ef63132	1280	}
jah128	2:c81f4ef63132	1281	} else {
jah128	2:c81f4ef63132	1282	swarm[sender].status_rf_request_reflected_ir = 4;
jah128	2:c81f4ef63132	1283	outcome = 4;
jah128	2:c81f4ef63132	1284	}
jah128	2:c81f4ef63132	1285	} else outcome = 2;
jah128	2:c81f4ef63132	1286	}
jah128	2:c81f4ef63132	1287	break;
jah128	2:c81f4ef63132	1288
jah128	2:c81f4ef63132	1289	case 12: { // Distance IR
jah128	2:c81f4ef63132	1290	if(swarm[sender].status_rf_request_distance_ir == 1) {
jah128	2:c81f4ef63132	1291	if(success == 1) {
jah128	2:c81f4ef63132	1292	if(length == 16) {
jah128	2:c81f4ef63132	1293	swarm[sender].status_rf_request_distance_ir = 2;
jah128	2:c81f4ef63132	1294	for(int i=0;i<8;i++){
jah128	2:c81f4ef63132	1295	int offset = i * 2;
jah128	2:c81f4ef63132	1296	unsigned short value = (unsigned short) data[offset] << 8;
jah128	2:c81f4ef63132	1297	value += data[offset+1];
jah128	2:c81f4ef63132	1298	float adjusted = (float) value / 655.0;
jah128	2:c81f4ef63132	1299	swarm[sender].distance_ir[i]=adjusted;
jah128	2:c81f4ef63132	1300	}
jah128	2:c81f4ef63132	1301	outcome = 1;
jah128	2:c81f4ef63132	1302	} else {
jah128	2:c81f4ef63132	1303	swarm[sender].status_rf_request_distance_ir = 3;
jah128	2:c81f4ef63132	1304	outcome = 3;
jah128	2:c81f4ef63132	1305	}
jah128	2:c81f4ef63132	1306	} else {
jah128	2:c81f4ef63132	1307	swarm[sender].status_rf_request_distance_ir = 4;
jah128	2:c81f4ef63132	1308	outcome = 4;
jah128	2:c81f4ef63132	1309	}
jah128	2:c81f4ef63132	1310	} else outcome = 2;
jah128	2:c81f4ef63132	1311	}
jah128	2:c81f4ef63132	1312	break;
jah128	2:c81f4ef63132	1313
jah128	2:c81f4ef63132	1314	case 13: { // Line following IR
jah128	2:c81f4ef63132	1315	if(swarm[sender].status_rf_request_line_following_ir == 1) {
jah128	2:c81f4ef63132	1316	if(success == 1) {
jah128	2:c81f4ef63132	1317	if(length == 10) {
jah128	2:c81f4ef63132	1318	swarm[sender].status_rf_request_line_following_ir = 2;
jah128	2:c81f4ef63132	1319	outcome = 1;
jah128	2:c81f4ef63132	1320	} else {
jah128	2:c81f4ef63132	1321	swarm[sender].status_rf_request_line_following_ir = 3;
jah128	2:c81f4ef63132	1322	outcome = 3;
jah128	2:c81f4ef63132	1323	}
jah128	2:c81f4ef63132	1324	} else {
jah128	2:c81f4ef63132	1325	swarm[sender].status_rf_request_line_following_ir = 4;
jah128	2:c81f4ef63132	1326	outcome = 4;
jah128	2:c81f4ef63132	1327	}
jah128	2:c81f4ef63132	1328	} else outcome = 2;
jah128	2:c81f4ef63132	1329	}
jah128	2:c81f4ef63132	1330	break;
jah128	2:c81f4ef63132	1331
jah128	2:c81f4ef63132	1332	case 14: { // Request uptime
jah128	2:c81f4ef63132	1333	if(swarm[sender].status_rf_request_uptime == 1) {
jah128	2:c81f4ef63132	1334	if(success == 1) {
jah128	2:c81f4ef63132	1335	if(length == 4) {
jah128	2:c81f4ef63132	1336	swarm[sender].status_rf_request_uptime = 2;
jah128	2:c81f4ef63132	1337	outcome = 1;
jah128	2:c81f4ef63132	1338	} else {
jah128	2:c81f4ef63132	1339	swarm[sender].status_rf_request_uptime = 3;
jah128	2:c81f4ef63132	1340	outcome = 3;
jah128	2:c81f4ef63132	1341	}
jah128	2:c81f4ef63132	1342	} else {
jah128	2:c81f4ef63132	1343	swarm[sender].status_rf_request_uptime = 4;
jah128	2:c81f4ef63132	1344	outcome = 4;
jah128	2:c81f4ef63132	1345	}
jah128	2:c81f4ef63132	1346	} else outcome = 2;
jah128	2:c81f4ef63132	1347	}
jah128	2:c81f4ef63132	1348	break;
jah128	2:c81f4ef63132	1349
jah128	2:c81f4ef63132	1350	}
jah128	2:c81f4ef63132	1351	} else {
jah128	2:c81f4ef63132	1352	// Response to a command
jah128	2:c81f4ef63132	1353	switch(function) {
jah128	2:c81f4ef63132	1354	case 0: {
jah128	2:c81f4ef63132	1355	if(swarm[sender].status_rf_command_stop == 1) {
jah128	2:c81f4ef63132	1356	if(success == 1){
jah128	2:c81f4ef63132	1357	if(length == 0) {
jah128	2:c81f4ef63132	1358	swarm[sender].status_rf_command_stop = 2;
jah128	2:c81f4ef63132	1359	outcome = 1;
jah128	2:c81f4ef63132	1360	} else {
jah128	2:c81f4ef63132	1361	swarm[sender].status_rf_command_stop = 3;
jah128	2:c81f4ef63132	1362	outcome = 3;
jah128	2:c81f4ef63132	1363	}
jah128	2:c81f4ef63132	1364	} else {
jah128	2:c81f4ef63132	1365	swarm[sender].status_rf_command_stop = 4;
jah128	2:c81f4ef63132	1366	outcome = 4;
jah128	2:c81f4ef63132	1367	}
jah128	2:c81f4ef63132	1368	} else outcome = 2;
jah128	2:c81f4ef63132	1369	}
jah128	2:c81f4ef63132	1370	break;
jah128	2:c81f4ef63132	1371	case 1: {
jah128	2:c81f4ef63132	1372	if(swarm[sender].status_rf_command_forward == 1) {
jah128	2:c81f4ef63132	1373	if(success == 1){
jah128	2:c81f4ef63132	1374	if(length == 0) {
jah128	2:c81f4ef63132	1375	swarm[sender].status_rf_command_forward = 2;
jah128	2:c81f4ef63132	1376	outcome = 1;
jah128	2:c81f4ef63132	1377	} else {
jah128	2:c81f4ef63132	1378	swarm[sender].status_rf_command_forward = 3;
jah128	2:c81f4ef63132	1379	outcome = 3;
jah128	2:c81f4ef63132	1380	}
jah128	2:c81f4ef63132	1381	} else {
jah128	2:c81f4ef63132	1382	swarm[sender].status_rf_command_forward = 4;
jah128	2:c81f4ef63132	1383	outcome = 4;
jah128	2:c81f4ef63132	1384	}
jah128	2:c81f4ef63132	1385	} else outcome = 2;
jah128	2:c81f4ef63132	1386	}
jah128	2:c81f4ef63132	1387	break;
jah128	2:c81f4ef63132	1388	case 2: {
jah128	2:c81f4ef63132	1389	if(swarm[sender].status_rf_command_backward == 1) {
jah128	2:c81f4ef63132	1390	if(success == 1){
jah128	2:c81f4ef63132	1391	if(length == 0) {
jah128	2:c81f4ef63132	1392	swarm[sender].status_rf_command_backward = 2;
jah128	2:c81f4ef63132	1393	outcome = 1;
jah128	2:c81f4ef63132	1394	} else {
jah128	2:c81f4ef63132	1395	swarm[sender].status_rf_command_backward = 3;
jah128	2:c81f4ef63132	1396	outcome = 3;
jah128	2:c81f4ef63132	1397	}
jah128	2:c81f4ef63132	1398	} else {
jah128	2:c81f4ef63132	1399	swarm[sender].status_rf_command_backward = 4;
jah128	2:c81f4ef63132	1400	outcome = 4;
jah128	2:c81f4ef63132	1401	}
jah128	2:c81f4ef63132	1402	} else outcome = 2;
jah128	2:c81f4ef63132	1403	}
jah128	2:c81f4ef63132	1404	break;
jah128	2:c81f4ef63132	1405	case 3: {
jah128	2:c81f4ef63132	1406	if(swarm[sender].status_rf_command_left == 1) {
jah128	2:c81f4ef63132	1407	if(success == 1){
jah128	2:c81f4ef63132	1408	if(length == 0) {
jah128	2:c81f4ef63132	1409	swarm[sender].status_rf_command_left = 2;
jah128	2:c81f4ef63132	1410	outcome = 1;
jah128	2:c81f4ef63132	1411	} else {
jah128	2:c81f4ef63132	1412	swarm[sender].status_rf_command_left = 3;
jah128	2:c81f4ef63132	1413	outcome = 3;
jah128	2:c81f4ef63132	1414	}
jah128	2:c81f4ef63132	1415	} else {
jah128	2:c81f4ef63132	1416	swarm[sender].status_rf_command_left = 4;
jah128	2:c81f4ef63132	1417	outcome = 4;
jah128	2:c81f4ef63132	1418	}
jah128	2:c81f4ef63132	1419	} else outcome = 2;
jah128	2:c81f4ef63132	1420	}
jah128	2:c81f4ef63132	1421	break;
jah128	2:c81f4ef63132	1422	case 4: {
jah128	2:c81f4ef63132	1423	if(swarm[sender].status_rf_command_right == 1) {
jah128	2:c81f4ef63132	1424	if(success == 1){
jah128	2:c81f4ef63132	1425	if(length == 0) {
jah128	2:c81f4ef63132	1426	swarm[sender].status_rf_command_right = 2;
jah128	2:c81f4ef63132	1427	outcome = 1;
jah128	2:c81f4ef63132	1428	} else {
jah128	2:c81f4ef63132	1429	swarm[sender].status_rf_command_right = 3;
jah128	2:c81f4ef63132	1430	outcome = 3;
jah128	2:c81f4ef63132	1431	}
jah128	2:c81f4ef63132	1432	} else {
jah128	2:c81f4ef63132	1433	swarm[sender].status_rf_command_right = 4;
jah128	2:c81f4ef63132	1434	outcome = 4;
jah128	2:c81f4ef63132	1435	}
jah128	2:c81f4ef63132	1436	} else outcome = 2;
jah128	2:c81f4ef63132	1437	}
jah128	2:c81f4ef63132	1438	break;
jah128	2:c81f4ef63132	1439	case 5: {
jah128	2:c81f4ef63132	1440	if(swarm[sender].status_rf_command_left_motor == 1) {
jah128	2:c81f4ef63132	1441	if(success == 1){
jah128	2:c81f4ef63132	1442	if(length == 0) {
jah128	2:c81f4ef63132	1443	swarm[sender].status_rf_command_left_motor = 2;
jah128	2:c81f4ef63132	1444	outcome = 1;
jah128	2:c81f4ef63132	1445	} else {
jah128	2:c81f4ef63132	1446	swarm[sender].status_rf_command_left_motor = 3;
jah128	2:c81f4ef63132	1447	outcome = 3;
jah128	2:c81f4ef63132	1448	}
jah128	2:c81f4ef63132	1449	} else {
jah128	2:c81f4ef63132	1450	swarm[sender].status_rf_command_left_motor = 4;
jah128	2:c81f4ef63132	1451	outcome = 4;
jah128	2:c81f4ef63132	1452	}
jah128	2:c81f4ef63132	1453	} else outcome = 2;
jah128	2:c81f4ef63132	1454	}
jah128	2:c81f4ef63132	1455	break;
jah128	2:c81f4ef63132	1456	case 6: {
jah128	2:c81f4ef63132	1457	if(swarm[sender].status_rf_command_right_motor == 1) {
jah128	2:c81f4ef63132	1458	if(success == 1){
jah128	2:c81f4ef63132	1459	if(length == 0) {
jah128	2:c81f4ef63132	1460	swarm[sender].status_rf_command_right_motor = 2;
jah128	2:c81f4ef63132	1461	outcome = 1;
jah128	2:c81f4ef63132	1462	} else {
jah128	2:c81f4ef63132	1463	swarm[sender].status_rf_command_right_motor = 3;
jah128	2:c81f4ef63132	1464	outcome = 3;
jah128	2:c81f4ef63132	1465	}
jah128	2:c81f4ef63132	1466	} else {
jah128	2:c81f4ef63132	1467	swarm[sender].status_rf_command_right_motor = 4;
jah128	2:c81f4ef63132	1468	outcome = 4;
jah128	2:c81f4ef63132	1469	}
jah128	2:c81f4ef63132	1470	} else outcome = 2;
jah128	2:c81f4ef63132	1471	}
jah128	2:c81f4ef63132	1472	break;
jah128	2:c81f4ef63132	1473	case 7: {
jah128	2:c81f4ef63132	1474	if(swarm[sender].status_rf_command_oled_colour == 1) {
jah128	2:c81f4ef63132	1475	if(success == 1){
jah128	2:c81f4ef63132	1476	if(length == 0) {
jah128	2:c81f4ef63132	1477	swarm[sender].status_rf_command_oled_colour = 2;
jah128	2:c81f4ef63132	1478	outcome = 1;
jah128	2:c81f4ef63132	1479	} else {
jah128	2:c81f4ef63132	1480	swarm[sender].status_rf_command_oled_colour = 3;
jah128	2:c81f4ef63132	1481	outcome = 3;
jah128	2:c81f4ef63132	1482	}
jah128	2:c81f4ef63132	1483	} else {
jah128	2:c81f4ef63132	1484	swarm[sender].status_rf_command_oled_colour = 4;
jah128	2:c81f4ef63132	1485	outcome = 4;
jah128	2:c81f4ef63132	1486	}
jah128	2:c81f4ef63132	1487	} else outcome = 2;
jah128	2:c81f4ef63132	1488	}
jah128	2:c81f4ef63132	1489	break;
jah128	2:c81f4ef63132	1490	case 8: {
jah128	2:c81f4ef63132	1491	if(swarm[sender].status_rf_command_cled_colour == 1) {
jah128	2:c81f4ef63132	1492	if(success == 1){
jah128	2:c81f4ef63132	1493	if(length == 0) {
jah128	2:c81f4ef63132	1494	swarm[sender].status_rf_command_cled_colour = 2;
jah128	2:c81f4ef63132	1495	outcome = 1;
jah128	2:c81f4ef63132	1496	} else {
jah128	2:c81f4ef63132	1497	swarm[sender].status_rf_command_cled_colour = 3;
jah128	2:c81f4ef63132	1498	outcome = 3;
jah128	2:c81f4ef63132	1499	}
jah128	2:c81f4ef63132	1500	} else {
jah128	2:c81f4ef63132	1501	swarm[sender].status_rf_command_cled_colour = 4;
jah128	2:c81f4ef63132	1502	outcome = 4;
jah128	2:c81f4ef63132	1503	}
jah128	2:c81f4ef63132	1504	} else outcome = 2;
jah128	2:c81f4ef63132	1505	}
jah128	2:c81f4ef63132	1506	break;
jah128	2:c81f4ef63132	1507	case 9: {
jah128	2:c81f4ef63132	1508	if(swarm[sender].status_rf_command_oled_state == 1) {
jah128	2:c81f4ef63132	1509	if(success == 1){
jah128	2:c81f4ef63132	1510	if(length == 0) {
jah128	2:c81f4ef63132	1511	swarm[sender].status_rf_command_oled_state = 2;
jah128	2:c81f4ef63132	1512	outcome = 1;
jah128	2:c81f4ef63132	1513	} else {
jah128	2:c81f4ef63132	1514	swarm[sender].status_rf_command_oled_state = 3;
jah128	2:c81f4ef63132	1515	outcome = 3;
jah128	2:c81f4ef63132	1516	}
jah128	2:c81f4ef63132	1517	} else {
jah128	2:c81f4ef63132	1518	swarm[sender].status_rf_command_oled_state = 4;
jah128	2:c81f4ef63132	1519	outcome = 4;
jah128	2:c81f4ef63132	1520	}
jah128	2:c81f4ef63132	1521	} else outcome = 2;
jah128	2:c81f4ef63132	1522	}
jah128	2:c81f4ef63132	1523	break;
jah128	2:c81f4ef63132	1524	case 10: {
jah128	2:c81f4ef63132	1525	if(swarm[sender].status_rf_command_cled_state == 1) {
jah128	2:c81f4ef63132	1526	if(success == 1){
jah128	2:c81f4ef63132	1527	if(length == 0) {
jah128	2:c81f4ef63132	1528	swarm[sender].status_rf_command_cled_state = 2;
jah128	2:c81f4ef63132	1529	outcome = 1;
jah128	2:c81f4ef63132	1530	} else {
jah128	2:c81f4ef63132	1531	swarm[sender].status_rf_command_cled_state = 3;
jah128	2:c81f4ef63132	1532	outcome = 3;
jah128	2:c81f4ef63132	1533	}
jah128	2:c81f4ef63132	1534	} else {
jah128	2:c81f4ef63132	1535	swarm[sender].status_rf_command_cled_state = 4;
jah128	2:c81f4ef63132	1536	outcome = 4;
jah128	2:c81f4ef63132	1537	}
jah128	2:c81f4ef63132	1538	} else outcome = 2;
jah128	2:c81f4ef63132	1539	}
jah128	2:c81f4ef63132	1540	break;
jah128	2:c81f4ef63132	1541	case 11: {
jah128	2:c81f4ef63132	1542	if(swarm[sender].status_rf_command_set_oled == 1) {
jah128	2:c81f4ef63132	1543	if(success == 1){
jah128	2:c81f4ef63132	1544	if(length == 0) {
jah128	2:c81f4ef63132	1545	swarm[sender].status_rf_command_set_oled = 2;
jah128	2:c81f4ef63132	1546	outcome = 1;
jah128	2:c81f4ef63132	1547	} else {
jah128	2:c81f4ef63132	1548	swarm[sender].status_rf_command_set_oled = 3;
jah128	2:c81f4ef63132	1549	outcome = 3;
jah128	2:c81f4ef63132	1550	}
jah128	2:c81f4ef63132	1551	} else {
jah128	2:c81f4ef63132	1552	swarm[sender].status_rf_command_set_oled = 4;
jah128	2:c81f4ef63132	1553	outcome = 4;
jah128	2:c81f4ef63132	1554	}
jah128	2:c81f4ef63132	1555	} else outcome = 2;
jah128	2:c81f4ef63132	1556	}
jah128	2:c81f4ef63132	1557	break;
jah128	2:c81f4ef63132	1558	case 12: {
jah128	2:c81f4ef63132	1559	if(swarm[sender].status_rf_command_play_tune == 1) {
jah128	2:c81f4ef63132	1560	if(success == 1){
jah128	2:c81f4ef63132	1561	if(length == 0) {
jah128	2:c81f4ef63132	1562	swarm[sender].status_rf_command_play_tune = 2;
jah128	2:c81f4ef63132	1563	outcome = 1;
jah128	2:c81f4ef63132	1564	} else {
jah128	2:c81f4ef63132	1565	swarm[sender].status_rf_command_play_tune = 3;
jah128	2:c81f4ef63132	1566	outcome = 3;
jah128	2:c81f4ef63132	1567	}
jah128	2:c81f4ef63132	1568	} else {
jah128	2:c81f4ef63132	1569	swarm[sender].status_rf_command_play_tune = 4;
jah128	2:c81f4ef63132	1570	outcome = 4;
jah128	2:c81f4ef63132	1571	}
jah128	2:c81f4ef63132	1572	} else outcome = 2;
jah128	2:c81f4ef63132	1573	}
jah128	2:c81f4ef63132	1574	break;
jah128	2:c81f4ef63132	1575	case 14: {
jah128	2:c81f4ef63132	1576	if(swarm[sender].status_rf_command_sync_time == 1) {
jah128	2:c81f4ef63132	1577	if(success == 1){
jah128	2:c81f4ef63132	1578	if(length == 0) {
jah128	2:c81f4ef63132	1579	swarm[sender].status_rf_command_sync_time = 2;
jah128	2:c81f4ef63132	1580	outcome = 1;
jah128	2:c81f4ef63132	1581	} else {
jah128	2:c81f4ef63132	1582	swarm[sender].status_rf_command_sync_time = 3;
jah128	2:c81f4ef63132	1583	outcome = 3;
jah128	2:c81f4ef63132	1584	}
jah128	2:c81f4ef63132	1585	} else {
jah128	2:c81f4ef63132	1586	swarm[sender].status_rf_command_sync_time = 4;
jah128	2:c81f4ef63132	1587	outcome = 4;
jah128	2:c81f4ef63132	1588	}
jah128	2:c81f4ef63132	1589	} else outcome = 2;
jah128	2:c81f4ef63132	1590	}
jah128	2:c81f4ef63132	1591	break;
jah128	2:c81f4ef63132	1592	}
jah128	2:c81f4ef63132	1593	}
jah128	2:c81f4ef63132	1594
jah128	2:c81f4ef63132	1595	if(RF_DEBUG) {
jah128	2:c81f4ef63132	1596	switch(outcome) {
jah128	2:c81f4ef63132	1597	case 0 :
jah128	2:c81f4ef63132	1598	pc.printf("Unknown RF response received");
jah128	2:c81f4ef63132	1599	case 1 :
jah128	2:c81f4ef63132	1600	pc.printf("RF response received, data updated.");
jah128	2:c81f4ef63132	1601	case 2 :
jah128	2:c81f4ef63132	1602	pc.printf("Unexpected RF response received, ignored.");
jah128	2:c81f4ef63132	1603	case 3 :
jah128	2:c81f4ef63132	1604	pc.printf("Invalid RF response received, ignored.");
jah128	2:c81f4ef63132	1605	case 4 :
jah128	2:c81f4ef63132	1606	pc.printf("RF response received: unsuccessful operation.");
jah128	2:c81f4ef63132	1607	}
jah128	2:c81f4ef63132	1608	}
jah128	2:c81f4ef63132	1609	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	14 Aug 2014
Imports:	2
Forks:	0
Commits:	6
Dependents:	0
Dependencies:	1
Followers:	1

communications.cpp@5:68a1ce96bfeb, 2014-08-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning