nav200.cc

机器人仿真软件

#include "nav200.h"


Nav200::Nav200()
{
}


Nav200::~Nav200()
{
}

int Nav200::Initialise(const char * port, int rate)
{
  bytesReceived = 0;
  fd = -1;

  // open the serial port
  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
  if ( fd<0 )
  {
    fprintf(stderr, "Could not open serial device %s\n",port);
    return -1;
  }

  // save the current io settings
  tcgetattr(fd, &oldtio);

  // set up new settings
  struct termios newtio;
  memset(&newtio, 0,sizeof(newtio));
  newtio.c_cflag = CS8 | CREAD | PARENB;
  newtio.c_iflag = INPCK;
  newtio.c_oflag = 0;
  newtio.c_lflag = 0;

  // activate new settings
  tcflush(fd, TCIFLUSH);
  if (cfsetispeed(&newtio, rate) < 0 || cfsetospeed(&newtio, rate) < 0)
  {
    fprintf(stderr,"Failed to set serial baud rate: %d\n", rate);
    tcsetattr(fd, TCSANOW, &oldtio);
    close(fd);
    fd = -1;
    return -1;
  }
  tcsetattr(fd, TCSANOW, &newtio);
  tcflush(fd, TCIOFLUSH);

  // clear the input buffer in case junk data is on the port
  usleep(10000);
  tcflush(fd, TCIFLUSH);
  return 0;
}

int Nav200::Terminate()
{
  // restore old port settings
  if (fd > 0)
  {
    tcsetattr(fd, TCSANOW, &oldtio);
    close(fd);
  }
  return 0;
}

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

int Nav200::GetVersionNumber()
{
  WriteCommand('S','V',0,NULL);
  if (ReadFromNav200() > 0)
  {
    if (packet.length==8 && packet.mode == 'S' && packet.function == 'V')
    {
      //printf("Version: %d.%d.%d\n",packet.data[0],packet.data[1],packet.data[2]);
      return (*reinterpret_cast<int*> (packet.data))&0xFFFFFF;
    }
  }
  return 0;

}

char* Nav200::GetVersionString()
{
  WriteCommand('S','T',0,NULL);
  if(ReadFromNav200()>0)
  {
    if(packet.mode == 'S' && packet.function == 'T')
    {
      packet.data[packet.dataLength]='\0';
      return(reinterpret_cast<char*>(packet.data));
    }
  }
  return NULL;
}

short Nav200::GetDeviceSerial()
{
  WriteCommand('S','S',0,NULL);
  if(ReadFromNav200()>0)
  {
    if(packet.length==7 && packet.mode == 'S' && packet.function == 'S')
      return *reinterpret_cast<short*> (packet.data);
  }
  return 0;
}

bool Nav200::rotateDirection(uint8_t direction)
{
  WriteCommand('S','U',1,&direction);
  if(ReadFromNav200()>0)
  {
    if(packet.length==6 && packet.mode == 'S' && packet.function == 'U' /*&& *reinterpret_cast<uint8_t*> (packet.data) == direction*/)
    {
      return true;
    }
  }
  return false;
}

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

bool Nav200::ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX, int newY)
{
  uint8_t command[10];
  command[0] = layer;
  command[1] = number;
  *reinterpret_cast<int*> (&command[2]) = newX;
  *reinterpret_cast<int*> (&command[6]) = newY;
  WriteCommand('S','C',10,command);
  if(ReadFromNav200()>0)
  {
    if(packet.length==15 && packet.mode == 'S' && packet.function == 'C' && packet.data[0]==layer && packet.data[1]==number && *reinterpret_cast<int*> (packet.data+2) == newX && *reinterpret_cast<int*> (packet.data+6) == newY)
      return true;
  }
  return false;
}

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

bool Nav200::DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector)
{
  uint8_t data[2] = {layer,number};
  WriteCommand('S','D',2,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==15 && packet.mode == 'S' && packet.function == 'D' && packet.data[0]==layer && packet.data[1]==number)
    {
        reflector.x = *reinterpret_cast<int*> (packet.data+2);
        reflector.y = *reinterpret_cast<int*> (packet.data+6);
//         printf("deleted reflector is %d\n",reflector.x);
        return true;
    }
  }
  return false;
}


/*********************************
* Read and set reflector radii
***********************************/
int Nav200::GetReflectorRadius(uint8_t layer)
{
//   ReflectorSize reflector;
  WriteCommand('R','G',1,&layer);
  if(ReadFromNav200()>0)
  {
    if(packet.length==7 && packet.mode == 'R' && packet.function == 'G' && packet.data[0] == layer)
    {
      return packet.data[1];
    }
  }
  return -1;
}

bool Nav200::SetReflectorRadius(uint8_t layer, uint8_t radius)
{
  uint8_t data[2] = {layer,radius};
  WriteCommand('R','S',2,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==7 && packet.mode == 'R' && packet.function == 'G' && packet.data[0] == layer && packet.data[1] == radius)
      return true;
  }
  return false;
}

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

int Nav200::StartMapping(uint8_t layer, int X, int Y, short orientation, uint8_t radius)
{
  uint8_t data[12];
  data[0] = layer;
  *reinterpret_cast<int*> (&data[1]) = X;
  *reinterpret_cast<int*> (&data[5]) = Y;
  *reinterpret_cast<short*> (&data[9]) = orientation;
  data[11] = radius;
  WriteCommand('M','S',12,data);
  if(ReadFromNav200(100000000)>0)
  {
    if(packet.length==7 && packet.mode == 'M' && packet.function == 'S' && packet.data[0] == layer)
      return packet.data[1];
  }
  return -1;
}

int Nav200::StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius)
{
  uint8_t data[13];
  data[0] = layer;
  data[1] = scans;
  *reinterpret_cast<int*> (&data[2]) = X;
  *reinterpret_cast<int*> (&data[6]) = Y;
  *reinterpret_cast<short*> (&data[10]) = orientation;
  data[12] = radius;
  WriteCommand('M','M',13,data);
  if(ReadFromNav200(100000000)>0)
  {
    if(packet.length==7 && packet.mode == 'M' && packet.function == 'S' && packet.data[0] == layer)
      return packet.data[1];
  }
  return -1;
}

int Nav200::StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius)
{
  uint8_t data[13];
  data[0] = layer;
  data[1] = scans;
  *reinterpret_cast<int*> (&data[2]) = X;
  *reinterpret_cast<int*> (&data[6]) = Y;
  *reinterpret_cast<short*> (&data[10]) = orientation;
  data[12] = radius;
  WriteCommand('M','N',13,data);
  if(ReadFromNav200()>0)
  {
    if(packet.length==7 && packet.mode == 'M' && packet.function == 'S' && packet.data[0] == layer)
      return packet.data[1];
  }
  return -1;
}

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

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

bool Nav200::EnterPositioningInput(uint8_t NumberOfMeasurements)
{
  WriteCommand('P','N',1,&NumberOfMeasurements);
  if(ReadFromNav200()>0)
  {
    if(packet.length==5 && packet.mode == 'P' && packet.function == 'A')
      return true;
  }
  return false;
}

bool Nav200::GetPositionAuto(LaserPos & laserPosition)
{
  WriteCommand('P','P',0,NULL);

  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::GetPositionSpeed(short speedX, short speedY, LaserPos & laserPosition)
{
  uint8_t data[4];
  *reinterpret_cast<short*> (&data[0]) = speedX;
  *reinterpret_cast<short*> (&data[2]) = speedY;
  WriteCommand('P','v',4,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::GetPositionSpeedVelocity(short speedX, short speedY, short velocity, LaserPos & laserPosition)
{
  uint8_t data[6];
  *reinterpret_cast<short*> (&data[0]) = speedX;