Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
communications.cpp
- Committer:
- jah128
- Date:
- 2014-01-31
- Revision:
- 0:9ffe8ebd1c40
- Child:
- 1:b067a08ff54e
File content as of revision 0:9ffe8ebd1c40:
/* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * * Version 0.4 January 2014 * * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 * */ #include "communications.h" #include "piswarm.h" #include "main.h" 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; const char * const requests_array[] = { "Req.Ack", "Req.LM", "Req.RM", "Req.But","Req.LEDCol","Req.LED","Req.Lgt","Req.Acc","Req.Gyro","Req.BIR","Req.RIR","Req.DIR","Req.LFS","Req.UpT","Req.???","Req.???"}; const char * const commands_array[] = { "Cm.Stop","Cm.Fward","Cm.Bward","Cm.Left","Cm.Right","Cm.LMot","Cm.RMot","Cm.OLEDC","Cm.CLEDC","Cm.OLEDS","Cm.CLEDS","Cm.SetLED","Cm.Play","Cm.Sync","Cm.???","Cm.???"}; //Decode the received message, action it if it is valid and for me void decodeMessage(char sender, char target, char id, char command, char * data, char length){ char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0; if(target==0) broadcast_message = 1; is_response = 0 != (command & (1 << 7)); request_response = 0 != (command & (1 << 6)); is_user = 0 != (command & (1 << 5)); is_command = 0 != (command & (1 << 4)); function = command % 16; if (RF_DEBUG==1) { if(is_command == 1) pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, commands_array[function],length); else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length); } //Action the message only if I am a recipient if(target==0 || target==piswarm.get_id()){ if(RF_USE_LEDS==1) actioning = 1; if(is_response == 1) { if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length); else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length); }else { if(is_command == 1) { if(RF_ALLOW_COMMANDS == 1){ if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length); else handle_command(sender, broadcast_message, request_response, id, function, data, length); } else if (RF_DEBUG==1) pc.printf(" - Blocked\n"); } else { //A information request has no extra parameters if(length == 0){ if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length); else handle_request(sender, broadcast_message, request_response, id, function); } else if (RF_DEBUG==1) pc.printf(" - Invalid\n"); } } if(RF_USE_LEDS==1) actioning = 0; } else if (RF_DEBUG==1) pc.printf(" - Ignored\n"); } //Send a 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 { piswarm.send_rf_message(message,4+length); if(RF_DEBUG==1)pc.printf("Response issued"); } } // Send a delayed response void tdma_response(){ 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 response void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){ } // Handle a message that is a (not-user) command void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length){ char success = 0; switch(function){ case 0: // Stop [0 data] if(length==0) { piswarm.stop(); if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - "); success = 1; } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 1: // Forward [2 bytes: 16-bit signed short] if(length==2) { int i_speed = (data[0] << 8) + data[1]; float speed = i_speed / 32768.0; speed--; piswarm.forward(speed); success = 1; if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 2: // Backward [2 bytes: 16-bit signed short] if(length==2) { int i_speed = (data[0] << 8) + data[1]; float speed = i_speed / 32768.0; speed--; piswarm.backward(speed); success = 1; if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 3: // Left [2 bytes: 16-bit signed short] if(length==2) { int i_speed = (data[0] << 8) + data[1]; float speed = i_speed / 32768.0; speed--; piswarm.left(speed); success = 1; if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 4: // Right [2 bytes: 16-bit signed short] if(length==2) { int i_speed = (data[0] << 8) + data[1]; float speed = i_speed / 32768.0; speed--; piswarm.right(speed); success = 1; if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 5: // Left Motor [2 bytes: 16-bit signed short] if(length==2) { int i_speed = (data[0] << 8) + data[1]; float speed = i_speed / 32768.0; speed--; piswarm.left_motor(speed); success = 1; if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 6: // Right Motor [2 bytes: 16-bit signed short] if(length==2) { int i_speed = (data[0] << 8) + data[1]; float speed = i_speed / 32768.0; speed--; piswarm.right_motor(speed); success = 1; if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 7: // Outer LED Colour [3 bytes: R, G, B] if(length==3) { piswarm.set_oled_colour (data[0],data[1],data[2]); success = 1; if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 8: // Center LED Colour[3 bytes: R, G, B] if(length==3) { piswarm.set_cled_colour (data[0],data[1],data[2]); success = 1; if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ] if(length==2) { piswarm.set_oleds(0 != (data[0] & (1 << 1)),0 != (data[0] & (1 << 0)),0 != (data[1] & (1 << 7)),0 != (data[1] & (1 << 6)),0 != (data[1] & (1 << 5)),0 != (data[1] & (1 << 4)),0 != (data[1] & (1 << 3)),0 != (data[1] & (1 << 2)),0 != (data[1] & (1 << 1)),0 != (data[1] & (1 << 0))); success = 1; } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ] if(length==1) { piswarm.enable_cled (data[0] % 2); success = 1; } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED] if(length==1) { int led = data[0] % 16; if(led < 10){ piswarm.set_oled(led, 0!=(data[0] & (1 << 4))); success = 1; } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 12: // Play sound [Minimum 1 byte] if(length>0) { piswarm.play_tune(data,length); success = 1; } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 13: // Sync time if(length==4) { unsigned int new_time = 0; new_time+=((unsigned int)data[0] << 24); new_time+=((unsigned int)data[1] << 16); new_time+=((unsigned int)data[2] << 8); new_time+=(unsigned int)data[3]; set_time(new_time); display_system_time(); } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); break; case 14: // break; case 15: // break; } if(request_response == 1){ send_response(sender, is_broadcast, success, id, 1, function, NULL, 0); } } //Handle a message that is a (not-user) 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(); 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); break; } case 5: // Request LED states 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; 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; break; } case 8: // Request accelerometer reading break; case 9: // Request gyroscope reading break; case 10: // Request background IR reading break; case 11: // Request illuminated IR reading break; case 12: // Request line-tracking IR reading break; case 13: // Request uptime break; case 14: // break; case 15: // break; } send_response(sender, is_broadcast, success, id, 0, function, response, response_length); } 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; } void errormessage(int index){ if(RF_USE_LEDS==1) errorled=1; switch(index){ case 0: //Message to short if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n"); break; case 1: //Sender out of valid range if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n"); break; case 2: //Target out of valid range if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n"); break; case 3: break; case 4: break; } }