nav200.cc

机器人仿真软件

  *reinterpret_cast<short*> (&data[2]) = speedY;
  *reinterpret_cast<short*> (&data[4]) = velocity;
  WriteCommand('P','w',6,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==17 && packet.mode == 'P' && packet.function == 'P')
    {
      laserPosition.pos.x = *reinterpret_cast<int*> (packet.data);
      laserPosition.pos.y = *reinterpret_cast<int*> (packet.data+4);
      laserPosition.orientation = *reinterpret_cast<short*> (packet.data+8);
      laserPosition.quality = packet.data[10];
      laserPosition.number = packet.data[11];
      return true;
    }
  }
  return false;
}


bool Nav200::GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short velocity, LaserPos & laserPosition)
{
  uint8_t data[6];
  *reinterpret_cast<short*> (&data[0]) = speedX;
  *reinterpret_cast<short*> (&data[2]) = speedY;
  *reinterpret_cast<short*> (&data[4]) = velocity;
  WriteCommand('P','V',6,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==17 && packet.mode == 'P' && packet.function == 'P')
    {
      laserPosition.pos.x = *reinterpret_cast<int*> (packet.data);
      laserPosition.pos.y = *reinterpret_cast<int*> (packet.data+4);
      laserPosition.orientation = *reinterpret_cast<short*> (packet.data+8);
      laserPosition.quality = packet.data[10];
      laserPosition.number = packet.data[11];
      return true;
    }
  }
  return false;
}

bool Nav200::ChangeLayer(uint8_t layer)
{
  WriteCommand('P','L',1,&layer);
  if(ReadFromNav200()>0)
  {
    if(packet.length==6 && packet.mode=='P' && packet.function=='L' && packet.data[0] == layer)
      return true;
  }
  return false;
}

bool Nav200::ChangeLayerDefPosition(uint8_t layer, int X, int Y, short orientation)
{
  uint8_t data[11];
  data[0] = layer;
  *reinterpret_cast<int*> (&data[1]) = X;
  *reinterpret_cast<int*> (&data[5]) = Y;
  *reinterpret_cast<short*> (&data[9]) = orientation;
  WriteCommand('P','M',11,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==16 && packet.mode == 'P' && packet.function == 'M' && packet.data[0] == layer && *reinterpret_cast<int*> (packet.data+1) == X && *reinterpret_cast<int*> (packet.data+5) == Y && *reinterpret_cast<short*> (packet.data+9) == orientation)
      return true;
  }
  return false;
}

bool Nav200::SetActionRadii(int min, int max)
{
  uint8_t data[8];
  *reinterpret_cast<int*> (&data[0]) = min;
  *reinterpret_cast<int*> (&data[4]) = max;
  WriteCommand('P','O',8,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==13 && packet.mode == 'P' && packet.function == 'O' && *reinterpret_cast<int*> (packet.data)==min && *reinterpret_cast<int*> (packet.data+4)==max)
      return true;
  }
  return false;
}

bool Nav200::SelectNearest(uint8_t N_nearest)
{
  WriteCommand('P','C',1,&N_nearest);
  if(ReadFromNav200()>0)
  {
    if(packet.length==6 && packet.mode == 'P' && packet.function == 'C' && packet.data[0] == N_nearest)
      return true;
  }
  return false;
}

/*****************
* upload mode
*******************/
bool Nav200::EnterUpload()
{
  WriteCommand('U','A',0,NULL);
  if(ReadFromNav200()>0)
  {
    if(packet.length==5 && packet.mode == 'U' && packet.function == 'A')
      return true;
  }
  return false;
}

bool Nav200::GetUploadTrans(uint8_t layer, ReflectorData & reflector)
{
  WriteCommand('U','R',1,&layer);
  if(ReadFromNav200()>0)
  {
    if(packet.length==15 && packet.mode == 'U' && packet.function == 'R' && packet.data[0] == layer)
    {
      reflector.layer = packet.data[0];
      reflector.number = packet.data[1];
      reflector.pos.x = *reinterpret_cast<int*> (packet.data+2);
      reflector.pos.y = *reinterpret_cast<int*> (packet.data+6);
      return true;
    }
  }
  return false;
}

/*****************
* download mode
******************/
bool Nav200::EnterDownload()
{
  WriteCommand('D','A',0,NULL);
  if(ReadFromNav200()>0)
  {
    if(packet.length==5 && packet.mode == 'D' && packet.function == 'A')
      return true;
  }
  return false;
}

bool Nav200::DownloadReflector(uint8_t layer, uint8_t number, int X, int Y)
{
  uint8_t data[10];
  data[0] = layer;
  data[1] = number;
  *reinterpret_cast<int*> (&data[2]) = X;
  *reinterpret_cast<int*> (&data[6]) = Y;
  WriteCommand('D','R',10,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length == 7 && packet.mode == 'D' && packet.function == 'R' && packet.data[0] == layer && packet.data[1] == number)
    {
      return true;
    }
  }
  return false;
}

/*****************
* Error Messages
******************/
void Nav200::PrintErrorMsg(void)
{
  fprintf(stderr,"ERROR: Last command entry %c:", error.F0); 
  switch(error.F1)
  {
    case 1:   fprintf(stderr,"User's software error: "); break;
    case 2:   fprintf(stderr,"Error class: Transputer: "); break;
    case 3:   fprintf(stderr,"Error in the connection between TPU and sensor: "); break;
    case 4:   fprintf(stderr,"Spurious measurement: "); break;
    case 5:   fprintf(stderr,"Spurious angular measurement: "); break;
    case 6:   fprintf(stderr,"Error in connection to reflector memory: "); break;
    case 7:   fprintf(stderr,"Error in connection between user computer and TPU (RS-232 interface): "); break;
  }
  switch(error.F2)
  {
    case 1:   fprintf(stderr,"input/output, telegram traffic. "); break;
    case 2:   fprintf(stderr,"standby mode. "); break;
    case 3:   fprintf(stderr,"handling reference reflectors. "); break;
    case 4:   fprintf(stderr,"download mode. "); break;
    case 5:   fprintf(stderr,"upload mode. "); break;
    case 6:   fprintf(stderr,"mapping mode. "); break;
    case 7:   fprintf(stderr,"positioning mode. "); break;
    case 8:   fprintf(stderr,"test mode. "); break;
    case 9:   fprintf(stderr,"navigation operation, general. "); break;

  }
  switch(error.F3)
  {
    case 1:   fprintf(stderr,"Unknown command.\n"); break;
    case 2:   fprintf(stderr,"Command(function) not implemented.\n"); break;
    case 3:   fprintf(stderr,"Wrong Command\n"); break;
    case 4:   fprintf(stderr,"Reflector not available\n"); break;
    case 5:   fprintf(stderr,"Wrong reflector number\n"); break;
    case 6:   fprintf(stderr,"Wrong data block\n"); break;
    case 7:   fprintf(stderr,"Input not possible\n"); break;
    case 8:   fprintf(stderr,"Invalid layer\n"); break;
    case 9:   fprintf(stderr,"No reflectors in layer\n"); break;
    case 10:   fprintf(stderr,"All reflectors transferred\n"); break;
    case 11:   fprintf(stderr,"Communication error\n"); break;
    case 12:   fprintf(stderr,"Error in the initialisation of reference reflectors\n"); break;
    case 13:   fprintf(stderr,"Wrong radii during input of effective range\n"); break;
    case 14:   fprintf(stderr,"Flash EPROM not functional\n"); break;
    case 15:   fprintf(stderr,"Wrong reflector radius\n"); break;
  }

}

////////////////////////////////////////////////////////////////////////////////
// Write a packet to Nav200
int Nav200::WriteCommand(char mode, char function, int dataLength, uint8_t * data)
{
  if (fd < 0)
    return -1;

  uint8_t * buffer;
  int length = dataLength+5;
  buffer = new uint8_t[length];

  // Create header
  buffer[0] = STX;
  buffer[1] = length;
  buffer[2] = mode;
  buffer[3] = function;

  // Copy data
  memcpy(buffer + 4, data, dataLength);

  // Create footer (CRC)
  buffer[length-1] = CreateCRC(buffer, 4 + dataLength);

  // Make sure both input and output queues are empty
  tcflush(fd, TCIOFLUSH);

  // switch to blocking IO for the write
  int flags = fcntl(fd, F_GETFL);
  if (flags < 0 || fcntl(fd,F_SETFL,flags &~O_NONBLOCK) < 0)
  {
    fprintf(stderr,"Error changing to blocking write (%d - %s), disabling\n",errno,strerror(errno));
    tcsetattr(fd, TCSANOW, &oldtio);
    fd = -1;
    return -1;
  }

  if((length && (write(fd, buffer, length)) < length))
  {
    fprintf(stderr,"Error writing to FOB (%d - %s), disabling\n",errno,strerror(errno));
    tcsetattr(fd, TCSANOW, &oldtio);
    fd = -1;
    return -1;
  }

  // restore flags
  if (fcntl(fd,F_SETFL,flags) < 0)
  {
    fprintf(stderr,"Error restoring file mode (%d - %s), disabling\n",errno,strerror(errno));
    tcsetattr(fd, TCSANOW, &oldtio);
    fd = -1;
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Read a packet from the Nav200
int Nav200::ReadFromNav200(int timeout_usec)
{
  int ret;
  int dataLength = 0;

  struct timeval start, now;
  gettimeofday(&start,NULL);
  
  for (;;)
  {
    gettimeofday(&now,NULL);
    if ((now.tv_sec - start.tv_sec) * 1e6 + now.tv_usec - start.tv_usec > timeout_usec)
    {
      fprintf(stderr,"Timed out waiting for packet %d uSecs passed\n",static_cast<int>((now.tv_sec - start.tv_sec) * 1e6 + now.tv_usec - start.tv_usec));
      return -1;
    }

    ret = read(fd, &receivedBuffer[bytesReceived], BUFFER_SIZE - bytesReceived);
    if (ret < 0)
    {
      fprintf(stderr,"Got error while reading %d %s\n",errno, strerror(errno));
      return ret;
    }
  
    bytesReceived += ret;

    if (ret == 0)
    {
      usleep(1000);
      continue;
    }
  
    // do we have enough for a header?
    while (bytesReceived > 4)
    {
      if (receivedBuffer[0] != STX)
      {
        // bad thing, we dont have the correct start to a message
        for(int i=1; i<bytesReceived; i++)
        { // find where STX is
          if(receivedBuffer[i]==STX)
          { // move so STX is at start
            memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i);
            bytesReceived-=i;
          }
        }
        continue;
      }
  
      // we have valid start byte
      // get length
      dataLength = receivedBuffer[1];
  
      // check we have enough data for full length, if not then break
      if (dataLength > bytesReceived)
      {
        break;
      }
      // calc CRC and check it
      // if bad CRC, assume same as bad STX
      if (CreateCRC(receivedBuffer, dataLength)) // change that later!!
      {// bad thing, we dont have the correct start to a message
        fprintf(stderr,"bad CRC!!!\n");
        for(int i=1; i<bytesReceived; i++)
        { // find where STX is
          if(receivedBuffer[i]==STX)
          { // move so STX is at start
            memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i);
            bytesReceived-=i;
          }
        }
        continue;
      }
      // if good CRC we actually have a packet so assemble our NAV200 message and return it to the application
      else
      {
        if(receivedBuffer[3]=='E'){
          error.F0 = receivedBuffer[4];
          error.F1 = receivedBuffer[5];
          error.F2 = receivedBuffer[6];
          error.F3 = receivedBuffer[7];
  
          //check out what the error is and it out
          PrintErrorMsg();
  
          return -2;
        }
  
        packet.header = receivedBuffer[0];
        packet.length = receivedBuffer[1];
        packet.mode = receivedBuffer[2];
        packet.function = receivedBuffer[3];
        packet.dataLength = packet.length-HEADER_SIZE-FOOTER_SIZE;
        memcpy(packet.data, receivedBuffer+4, packet.dataLength);
        packet.BCC = receivedBuffer[4+packet.dataLength];
  
        memmove(receivedBuffer, receivedBuffer+dataLength, bytesReceived-dataLength);
        bytesReceived-=dataLength;
        return 1;
      }
    }
  }
  return 0;
}


uint8_t Nav200::CreateCRC(uint8_t* data, ssize_t len)
{
  uint8_t result = 0;
  for (int ii = 0; ii < len; ++ii)
    result ^= data[ii];
  return result;
}