Unfinished version 0.6 library for the Pi Swarm robot. NOTE: This library is not yet finished or fully tested - it will change.
Dependents: Pi_Swarm_Blank Aggregation-Flocking_2 Pi_Swarm_User_Command_RF_Test
Fork of Pi_Swarm_Library by
Diff: communications.cpp
- Revision:
- 2:0a739218ab11
- Parent:
- 1:b067a08ff54e
- Child:
- 3:4c0f2f3de33e
--- a/communications.cpp Sun Feb 02 18:05:58 2014 +0000 +++ b/communications.cpp Sun Feb 02 20:37:48 2014 +0000 @@ -26,11 +26,17 @@ 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(); - message[1]=target; + message[0]=piswarm.get_id() + 32; + message[1]=target + 32; message_id++; message[2]=message_id % 256; message[3]=command; @@ -47,7 +53,7 @@ // 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 +// 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) { @@ -75,7 +81,7 @@ if(RF_USE_LEDS==1) rx=0; } -//Decode the received message, action it if it is valid and for me [called in alpha433.cpp] +//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; @@ -116,7 +122,7 @@ } else if (RF_DEBUG==1) pc.printf(" - Ignored\n"); } -//Send a response message +//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]; @@ -378,7 +384,20 @@ 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; break; } case 9: { // Request gyroscope reading @@ -432,253 +451,7 @@ -// Handle a message that is a response -void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length) -{ - 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(length == 0) { - swarm[sender].status_rf_request_null = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_null= 3; - outcome = 3; - } - } else outcome = 2; - } - break; - case 1: { //Left motor speed - if(swarm[sender].status_rf_request_left_motor_speed == 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 outcome = 2; - } - break; - - case 2: { //Right motor speed - if(swarm[sender].status_rf_request_right_motor_speed == 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 outcome = 2; - } - break; - - case 3: { //Button state - if(swarm[sender].status_rf_request_button_state == 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 outcome = 2; - } - break; - - case 4: { //LED Colour - if(swarm[sender].status_rf_request_led_colour == 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 outcome = 2; - } - break; - - case 5: { //LED States - if(swarm[sender].status_rf_request_led_states == 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 outcome = 2; - } - break; - - - case 6: { //Battery - if(swarm[sender].status_rf_request_battery == 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 outcome = 2; - } - break; - - case 7: { //Light sensor - if(swarm[sender].status_rf_request_light_sensor == 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 outcome = 2; - } - break; - - case 8: { //Accelerometer - if(swarm[sender].status_rf_request_accelerometer == 1) { - if(length == 6) { - swarm[sender].status_rf_request_accelerometer = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_accelerometer = 3; - outcome = 3; - } - } else outcome = 2; - } - break; - - case 9: { //Gyroscope - if(swarm[sender].status_rf_request_gyroscope == 1) { - if(length == 2) { - swarm[sender].status_rf_request_gyroscope = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_gyroscope = 3; - outcome = 3; - } - } else outcome = 2; - } - break; - - case 10: { //Background IR - if(swarm[sender].status_rf_request_background_ir == 1) { - if(length == 16) { - swarm[sender].status_rf_request_background_ir = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_background_ir = 3; - outcome = 3; - } - } else outcome = 2; - } - break; - - case 11: { //Reflected IR - if(swarm[sender].status_rf_request_background_ir == 1) { - if(length == 16) { - swarm[sender].status_rf_request_background_ir = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_background_ir = 3; - outcome = 3; - } - } else outcome = 2; - } - break; - - case 12: { // Distance IR - if(swarm[sender].status_rf_request_distance_ir == 1) { - if(length == 16) { - swarm[sender].status_rf_request_distance_ir = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_distance_ir = 3; - outcome = 3; - } - } else outcome = 2; - } - break; - - case 13: { // Line following IR - if(swarm[sender].status_rf_request_line_following_ir == 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 outcome = 2; - } - break; - - case 14: { // Request uptime - if(swarm[sender].status_rf_request_uptime == 1) { - if(length == 4) { - swarm[sender].status_rf_request_uptime = 2; - outcome = 1; - } else { - swarm[sender].status_rf_request_uptime = 3; - outcome = 3; - } - } else outcome = 2; - } - break; - - } - } else { - // Response to a command - } - - if(RF_DEBUG) { - switch(outcome) { - case 0 : - pc.printf("Unknown RF response received"); - case 1 : - pc.printf("RF response received, data updated."); - case 2 : - pc.printf("Unexpected RF response received, ignored."); - case 3 : - pc.printf("Invalid RF response received, ignored."); - } - } -} void send_rf_request_null ( char target ) { @@ -1212,3 +985,577 @@ swarm[i].gyro = 0.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; + 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; + 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; + outcome = 1; + } else { + swarm[sender].status_rf_request_distance_ir = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_request_distance_ir = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + + case 13: { // Line following IR + if(swarm[sender].status_rf_request_line_following_ir == 1) { + if(success == 1) { + if(length == 10) { + swarm[sender].status_rf_request_line_following_ir = 2; + outcome = 1; + } else { + swarm[sender].status_rf_request_line_following_ir = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_request_line_following_ir = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + + case 14: { // Request uptime + if(swarm[sender].status_rf_request_uptime == 1) { + if(success == 1) { + if(length == 4) { + swarm[sender].status_rf_request_uptime = 2; + outcome = 1; + } else { + swarm[sender].status_rf_request_uptime = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_request_uptime = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + + } + } else { + // Response to a command + switch(function) { + case 0: { + if(swarm[sender].status_rf_command_stop == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_stop = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_stop = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_stop = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 1: { + if(swarm[sender].status_rf_command_forward == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_forward = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_forward = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_forward = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 2: { + if(swarm[sender].status_rf_command_backward == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_backward = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_backward = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_backward = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 3: { + if(swarm[sender].status_rf_command_left == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_left = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_left = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_left = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 4: { + if(swarm[sender].status_rf_command_right == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_right = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_right = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_right = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 5: { + if(swarm[sender].status_rf_command_left_motor == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_left_motor = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_left_motor = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_left_motor = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 6: { + if(swarm[sender].status_rf_command_right_motor == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_right_motor = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_right_motor = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_right_motor = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 7: { + if(swarm[sender].status_rf_command_oled_colour == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_oled_colour = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_oled_colour = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_oled_colour = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 8: { + if(swarm[sender].status_rf_command_cled_colour == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_cled_colour = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_cled_colour = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_cled_colour = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 9: { + if(swarm[sender].status_rf_command_oled_state == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_oled_state = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_oled_state = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_oled_state = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 10: { + if(swarm[sender].status_rf_command_cled_state == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_cled_state = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_cled_state = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_cled_state = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 11: { + if(swarm[sender].status_rf_command_set_oled == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_set_oled = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_set_oled = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_set_oled = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 12: { + if(swarm[sender].status_rf_command_play_tune == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_play_tune = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_play_tune = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_play_tune = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + case 14: { + if(swarm[sender].status_rf_command_sync_time == 1) { + if(success == 1){ + if(length == 0) { + swarm[sender].status_rf_command_sync_time = 2; + outcome = 1; + } else { + swarm[sender].status_rf_command_sync_time = 3; + outcome = 3; + } + } else { + swarm[sender].status_rf_command_sync_time = 4; + outcome = 4; + } + } else outcome = 2; + } + break; + } + } + + if(RF_DEBUG) { + switch(outcome) { + case 0 : + pc.printf("Unknown RF response received"); + case 1 : + pc.printf("RF response received, data updated."); + case 2 : + pc.printf("Unexpected RF response received, ignored."); + case 3 : + pc.printf("Invalid RF response received, ignored."); + case 4 : + pc.printf("RF response received: unsuccessful operation."); + } + } +} \ No newline at end of file