Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
communications.cpp
- Committer:
- jah128
- Date:
- 2014-02-18
- Revision:
- 9:7a4fc1d7e484
- Parent:
- 4:52b3e4c5a425
- Child:
- 11:5ebcb52726cf
File content as of revision 9:7a4fc1d7e484:
/******************************************************************************************* * * University of York Robot Lab Pi Swarm Library: Swarm Communications Handler * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * * Version 0.6 February 2014 * * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 * ******************************************************************************************/ // Important note: The communication stack is enabled by setting the "USE_COMMUNICATION_STACK" flag to 1 // When being used, all received messages are decoded using the decodeMessage() function // See manual for more info on using the communication handler #include "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; } } }