www.pudn.com > Product_Submit2004.rar > Comms.cc, change:2004-06-04,size:6097b


#include "NUBOT.h" 
 
int NUBOT::ConstructUnclassifiedImagePacket(byte* data) { 
  byte* p = data; 
  int packetOffset = 0; 
 
  memcpy(p+packetOffset,visionSystem.GetUnclassified(), RAW_IMAGE_SIZE); 
  packetOffset += RAW_IMAGE_SIZE; 
 
#ifdef ERS_7 
  memcpy(p+packetOffset,&sensorValues_[S_HEAD_TILT1],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(p+packetOffset,&sensorValues_[S_HEAD_PAN],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(p+packetOffset,&sensorValues_[S_HEAD_TILT2],sizeof(long)); 
  packetOffset += sizeof(long); 
#endif 
 
#ifdef ERS_210 
  memcpy(p+packetOffset,&sensorValues_[S_HEAD_TILT],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(p+packetOffset,&sensorValues_[S_HEAD_PAN],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(p+packetOffset,&sensorValues_[S_HEAD_ROLL],sizeof(long)); 
  packetOffset += sizeof(long); 
#endif 
 
  memcpy(p+packetOffset,&averagedAccData[0],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(p+packetOffset,&averagedAccData[1],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(p+packetOffset,&averagedAccData[2],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  return packetOffset; 
} 
 
int NUBOT::ConstructClassifiedImagePacket(byte* data) { 
  int packetOffset = 0; 
  if (currentImageWidth_ == FULL_IMAGE_WIDTH) { 
    memcpy(data,"F10",3); 
  } else if (currentImageWidth_ == DOUBLE_IMAGE_WIDTH) { 
    memcpy(data,"D10",3); 
  } 
  packetOffset+=3; 
  memcpy(data+packetOffset,visionSystem.GetClassified(), currentImageWidth_*currentImageHeight_); 
  packetOffset += currentImageWidth_*currentImageHeight_; 
 
#ifdef ERS_7 
  memcpy(data+packetOffset,&previousSensorValues_[S_HEAD_TILT1],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&previousSensorValues_[S_HEAD_PAN],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&previousSensorValues_[S_HEAD_TILT2],sizeof(long)); 
  packetOffset += sizeof(long); 
#endif 
#ifdef ERS_210 
  memcpy(data+packetOffset,&previousSensorValues_[S_HEAD_TILT],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&previousSensorValues_[S_HEAD_PAN],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&previousSensorValues_[S_HEAD_ROLL],sizeof(long)); 
  packetOffset += sizeof(long); 
#endif 
 
/* 
  memcpy(data+packetOffset,&previousSensorValues_[S_ACCEL_FOR],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&previousSensorValues_[S_ACCEL_SIDE],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&previousSensorValues_[S_ACCEL_Z],sizeof(long)); 
  packetOffset += sizeof(long);*/ 
 
  memcpy(data+packetOffset,&averagedAccData[0],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&averagedAccData[1],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  memcpy(data+packetOffset,&averagedAccData[2],sizeof(long)); 
  packetOffset += sizeof(long); 
 
  return packetOffset; 
} 
 
int NUBOT::ConstructTeamPacket(byte* data) { 
  int bvrIncrease = 0; 
  int numMessages = 1; 
  if (outMessageSize_ > 0) { // I.e. we have some bvr messages to send 
    bvrIncrease = sizeof(byte) + sizeof(int) + outMessageSize_; 
    numMessages = 2; 
  } 
  int messageSize = numWorldObjects_*sizeof(WorldObject);  
  byte message[sizeof(byte)*3+sizeof(int)+messageSize + bvrIncrease]; 
  int count = 0; 
  message[count] = (byte)BOTID_; 
  count++;     
  message[count] = (byte)numMessages; // just one message included. 
  count++; 
  message[count] = (byte)1; // message type == WM 
  count++; 
  memcpy(message+count, &messageSize, sizeof(int)); 
  count+=sizeof(int); 
  memcpy(message+count, worldObjects_, messageSize); 
  count+=messageSize; 
  if (outMessageSize_ > 0) { // I.e. we have some bvr messages to send 
    message[count] = (byte)2; // message type == BVR 
    count++; 
    memcpy(message+count, &outMessageSize_, sizeof(int)); 
    count+=sizeof(int); 
    memcpy(message+count, outMessage_, outMessageSize_); 
    count+=outMessageSize_; 
  }      
  outMessageSize_ = 0;      // Reset the behaviour message 
 
  int packetSize = sizeof(byte)*3+sizeof(int)+messageSize + bvrIncrease; 
  memcpy(data,&message,packetSize); 
  return packetSize; 
} 
 
void NUBOT::InterpretTeamPacket(byte* data) { 
  byte* message = data; 
  byte messageSource = *message; 
  message++; 
  byte numMessages = *message; 
  message++; 
  byte currentMessage = 0; 
  while (currentMessage < numMessages) { 
    byte messageType = *message; 
    message++; 
    int messageSize; 
    memcpy(&messageSize, message, sizeof(int)); 
    message+=sizeof(int); 
    if (messageType == 1) { // world model message 
      int slot = -1; 
      for (int i = 0; i < WM_BUFFERSIZE; i++) { 
        if (toWMMessages[i].freeSlot == true) { 
          slot = i; 
          break; 
        } 
      } 
      if (slot == -1) { 
        cout << "Gateway Comms: No free toWM slots." << endl << flush; 
        break; // breaks out of while loop 
      } 
      toWMMessages[slot].freeSlot=false; 
      toWMMessages[slot].numWorldObjects = messageSize / sizeof(WorldObject); 
      toWMMessages[slot].size = messageSize; 
      toWMMessages[slot].sourceBotID = messageSource; 
      memcpy(toWMMessages[slot].data, message, messageSize); 
       // slot is interpreted then cleared in IncWirelessWM (nubot.cc) 
    } else if (messageType == 2) {  // bvr message 
      int slot = -1; 
      for (int i = 0; i < BVR_BUFFERSIZE; i++) { 
        if (toBVRMessages[i].freeSlot == true) { 
          slot = i; 
          break; 
        } 
      } 
      if (slot == -1) { 
        cout << "Gateway Comms: No free toBvr slots." << endl << flush; 
        break; // breaks out of while loop 
      } 
      toBVRMessages[slot].freeSlot=false; 
      toBVRMessages[slot].sourceBotID = messageSource; 
      toBVRMessages[slot].size = messageSize; 
      memcpy(toBVRMessages[slot].data, message, messageSize); 
    } 
    message+=messageSize; 
    currentMessage++; 
  }  
}