amtecpowercube.cc

机器人仿真软件

    rcmd[ctr++] = 0x10;
    rcmd[ctr++] = 0x90;
    break;
  default:
    rcmd[ctr++] = bcc;
  }
  rcmd[ctr++] = AMTEC_ETX;

  if(WriteData(rcmd, ctr) == ctr)
    return(0);
  else
  {
    PLAYER_ERROR("short write");
    return(-1);
  }
}

int
AmtecPowerCube::WriteData(unsigned char *buf, size_t len)
{
  size_t written = 0;
  int tmp = 0;

  while(written < len)
  {
    if((tmp = write(fd, buf, len)) < 0)
    {
      PLAYER_ERROR1("write() failed: %s", strerror(errno));
      return(-1);
    }

    written += tmp;
  }
  return(written);
}

int
AmtecPowerCube::AwaitETX(unsigned char* buf, size_t len)
{
  int pos, loop, numread, totalnumread;
  pos = 0; loop = 0;
  while(loop<10)
  {
    if((numread = read(fd,buf+pos,len-pos)) < 0)
    {
      PLAYER_ERROR1("read() failed:%s", strerror(errno));
      return(-1);
    }
    else if(!numread)
    {
      if(!fd_blocking)
        usleep(10000);
      loop++;
    }
    else
    {
      if(buf[pos+numread-1]==AMTEC_ETX)
      {
	totalnumread = pos+numread-1;
	return(totalnumread);
      }
      pos += numread;
    }
  }
  PLAYER_ERROR("never found ETX");
  return(-1);
}

int
AmtecPowerCube::AwaitAnswer(unsigned char* buf, size_t len)
{
  int numread;

  // if we're not blocking, give the unit some time to respond
  if(!fd_blocking)
    usleep(AMTEC_SLEEP_TIME_USEC);

  for(;;)
  {
    if((numread = read(fd, buf, 1)) < 0)
    {
      PLAYER_ERROR1("read() failed:%s", strerror(errno));
      return(-1);
    }
    else if(!numread)
    {
      // hmm...we were expecting something, yet we read
      // zero bytes. some glitch.  drain input, and return
      // zero.  we'll get a message next time through.
      PLAYER_WARN("read 0 bytes");
      if(tcflush(fd, TCIFLUSH ) < 0 )
      {
        PLAYER_ERROR1("tcflush() failed:%s",strerror(errno));
        return(-1);
      }
      return(0);
    }
    else
    {
      if(buf[0]==AMTEC_STX) 
        return(AwaitETX(buf,len));
      else
        continue;
    }
  }
}

size_t
AmtecPowerCube::ConvertBuffer(unsigned char* buf, size_t len)
{
  size_t i, j, actual_len;

  actual_len = len;

  for (i=0;i<len;i++) 
  {
    if(buf[i]==AMTEC_DLE) 
    {
      switch(buf[i+1]) 
      {
        case 0x82:
          buf[i] = 0x02;
          for(j=i+2;j<len;j++) 
            buf[j-1] = buf[j];
          actual_len--;
          break;
        case 0x83:
          buf[i] = 0x03;
          for(j=i+2;j<len;j++) 
            buf[j-1] = buf[j];
          actual_len--;
          break;
        case 0x90:
          buf[i] = 0x10;
          for(j=i+2;j<len;j++) 
            buf[j-1] = buf[j];
          actual_len--;
          break;
      }
    }
  }
  return(actual_len);
}

int
AmtecPowerCube::ReadAnswer(unsigned char* buf, size_t len)
{
  int actual_len;

  if((actual_len = AwaitAnswer(buf, len)) <= 0)
    return(actual_len);
  else
    return((int)ConvertBuffer(buf, (size_t)actual_len));
}
// The preceding methods are based some found in CARMEN.  Thanks to the
// authors.
////////////////////////////////////////////////////////////////////////////

int
AmtecPowerCube::GetFloatParam(int id, int param, float* val)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[2];

  cmd[0] = AMTEC_CMD_GET_EXT;
  cmd[1] = param;

  if(SendCommand(id, cmd, 2) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }

  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }

  *val = BytesToFloat(buf+4);
  return(0);
}

int
AmtecPowerCube::GetUint32Param(int id, int param, unsigned int* val)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[2];

  cmd[0] = AMTEC_CMD_GET_EXT;
  cmd[1] = param;

  if(SendCommand(id, cmd, 2) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }

  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }

  *val = BytesToUint32(buf+4);
  return(0);
}

int
AmtecPowerCube::SetFloatParam(int id, int param, float val)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[6];

  cmd[0] = AMTEC_CMD_SET_EXT;
  cmd[1] = param;
  FloatToBytes(cmd+2, val);

  if(SendCommand(id, cmd, 6) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }

  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }

  return(0);
}

int
AmtecPowerCube::GetPanTiltPos(short* pan, short* tilt)
{
  float tmp;

  // get the pan
  if(GetFloatParam(AMTEC_MODULE_PAN, AMTEC_PARAM_ACT_POS, &tmp) < 0)
  {
    PLAYER_ERROR("GetFloatParam() failed");
    return(-1);
  }
  // reverse pan angle, to increase ccw, then normalize
  *pan = -(short)RTOD(NORMALIZE(tmp));
  
  // get the tilt
  if(GetFloatParam(AMTEC_MODULE_TILT, AMTEC_PARAM_ACT_POS, &tmp) < 0)
  {
    PLAYER_ERROR("GetFloatParam() failed");
    return(-1);
  }
  *tilt = (short)(RTOD(tmp));

  return(0);
}

int
AmtecPowerCube::GetPanTiltVel(short* panspeed, short* tiltspeed)
{
  float tmp;

  // get the pan
  if(GetFloatParam(AMTEC_MODULE_PAN, AMTEC_PARAM_ACT_VEL, &tmp) < 0)
  {
    PLAYER_ERROR("GetFloatParam() failed");
    return(-1);
  }
  // reverse pan angle, to increase ccw, then normalize
  *panspeed = -(short)RTOD(NORMALIZE(tmp));
  
  // get the tilt
  if(GetFloatParam(AMTEC_MODULE_TILT, AMTEC_PARAM_ACT_VEL, &tmp) < 0)
  {
    PLAYER_ERROR("GetFloatParam() failed");
    return(-1);
  }
  *tiltspeed = (short)(RTOD(tmp));

  return(0);
}

int
AmtecPowerCube::SetLimits()
{
  // have to reverse the signs for the pan limits, since the Amtec unit
  // counts up clockwise, rather than ccw.
  if(this->minpan != INT_MAX)
  {
    if(SetFloatParam(AMTEC_MODULE_PAN, AMTEC_PARAM_MAX_POS, 
                     NORMALIZE(DTOR(-this->minpan))) < 0)
    {
      PLAYER_ERROR("SetFloatParam() failed");
      return(-1);
    }
  }
  if(this->maxpan != INT_MAX)
  {
    if(SetFloatParam(AMTEC_MODULE_PAN, AMTEC_PARAM_MIN_POS, 
                     NORMALIZE(DTOR(-this->maxpan))) < 0)
    {
      PLAYER_ERROR("SetFloatParam() failed");
      return(-1);
    }
  }
  if(this->mintilt != INT_MAX)
  {
    if(SetFloatParam(AMTEC_MODULE_TILT, AMTEC_PARAM_MIN_POS, 
                     NORMALIZE(DTOR(this->mintilt))) < 0)
    {
      PLAYER_ERROR("SetFloatParam() failed");
      return(-1);
    }
  }
  if(this->maxtilt != INT_MAX)
  {
    if(SetFloatParam(AMTEC_MODULE_TILT, AMTEC_PARAM_MAX_POS, 
                     NORMALIZE(DTOR(this->maxtilt))) < 0)
    {
      PLAYER_ERROR("SetFloatParam() failed");
      return(-1);
    }
  }
  return(0);
}

int
AmtecPowerCube::SetPanPos(short oldpan, short pan)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[8];
  float newpan;
  unsigned short time;

  newpan = DTOR(pan);
  time = (unsigned short)rint(((double)abs(pan - oldpan) / 
                               (double)this->speed) * 1e3);

  cmd[0] = AMTEC_CMD_SET_MOTION;
  cmd[1] = AMTEC_MOTION_FSTEP_ACK;
  FloatToBytes(cmd+2,newpan);
  Uint16ToBytes(cmd+6,time);
  if(SendCommand(AMTEC_MODULE_PAN,cmd,8) < 0)
    return(-1);
  if(ReadAnswer(buf,sizeof(buf)) < 0)
    return(-1);
  return(0);
}

int
AmtecPowerCube::SetTiltPos(short oldtilt, short tilt)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[8];
  float newtilt;
  unsigned short time;

  newtilt = DTOR(tilt);
  time = (unsigned short)rint(((double)abs(tilt - oldtilt) / 
                               (double)this->speed) * 1e3);

  cmd[0] = AMTEC_CMD_SET_MOTION;
  cmd[1] = AMTEC_MOTION_FSTEP_ACK;
  FloatToBytes(cmd+2,newtilt);
  Uint16ToBytes(cmd+6,time);
  if(SendCommand(AMTEC_MODULE_TILT,cmd,8) < 0)
    return(-1);
  if(ReadAnswer(buf,sizeof(buf)) < 0)
    return(-1);
  return(0);
}

int
AmtecPowerCube::SetPanVel(short panspeed)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[6];
  float newpanspeed;

  newpanspeed = DTOR(panspeed);

  cmd[0] = AMTEC_CMD_SET_MOTION;
  cmd[1] = AMTEC_MOTION_FVEL_ACK;
  FloatToBytes(cmd+2,newpanspeed);
  printf("sending pan command: %d (%f)\n", panspeed, newpanspeed);
  if(SendCommand(AMTEC_MODULE_PAN,cmd,6) < 0)
    return(-1);
  if(ReadAnswer(buf,sizeof(buf)) < 0)
    return(-1);
  return(0);
}

int
AmtecPowerCube::SetTiltVel(short tiltspeed)
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[6];
  float newtiltspeed;

  newtiltspeed = DTOR(tiltspeed);

  cmd[0] = AMTEC_CMD_SET_MOTION;
  cmd[1] = AMTEC_MOTION_FVEL_ACK;
  FloatToBytes(cmd+2,newtiltspeed);
  if(SendCommand(AMTEC_MODULE_TILT,cmd,6) < 0)
    return(-1);
  if(ReadAnswer(buf,sizeof(buf)) < 0)
    return(-1);
  return(0);
}


int AmtecPowerCube::ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr, void * data)
{
  assert(hdr);
  assert(data);

	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, device_addr))
	{
      short newpan, newtilt;

      short newpanspeed, newtiltspeed;
	  assert(hdr->size == sizeof(player_ptz_cmd_t));
	  player_ptz_cmd_t & command = *reinterpret_cast<player_ptz_cmd_t *> (data);
  
      if(this->controlmode == PLAYER_PTZ_POSITION_CONTROL)
      {
        // reverse pan angle, to increase ccw
        newpan = -(short)(command.pan);
        newtilt = (short)(command.tilt);

        if(newpan != lastpan)
        {
          // send new pan position
          if(SetPanPos(lastpan,newpan))
          {
            PLAYER_ERROR("SetPan() failed(); bailing.");
            pthread_exit(NULL);
          }

          lastpan = newpan;
        }

        if(newtilt != lasttilt)
        {
          // send new tilt position
          if(SetTiltPos(lasttilt,newtilt))
          {
            PLAYER_ERROR("SetTilt() failed(); bailing.");
            pthread_exit(NULL);
          }

          lasttilt = newtilt;
        }
      }
      else if(this->controlmode == PLAYER_PTZ_VELOCITY_CONTROL)
      {
        // reverse pan angle, to increase ccw
        newpanspeed = -(short)(command.panspeed);
        newtiltspeed = (short)(command.tiltspeed);

        if(newpanspeed != lastpanspeed)
        {
          // send new pan speed
          if(SetPanVel(newpanspeed))
          {
            PLAYER_ERROR("SetPanVel() failed(); bailing.");
            pthread_exit(NULL);
          }

          lastpanspeed = newpanspeed;
         }

        if(newtiltspeed != lasttiltspeed)
        {
          // send new tilt position
          if(SetTiltVel(newtiltspeed))
          {
            PLAYER_ERROR("SetTiltVel() failed(); bailing.");
            pthread_exit(NULL);
          }

          lasttiltspeed = newtiltspeed;
        }
      }
      else
      {
        PLAYER_ERROR1("unkown control mode: %d; bailing",this->controlmode);
         pthread_exit(NULL);
      }

  	  return 0;
	}

	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_PTZ_REQ_CONTROL_MODE, device_addr))
	{
	  player_ptz_req_control_mode_t * cfg = reinterpret_cast<player_ptz_req_control_mode_t *> (data);

      if((cfg->mode != PLAYER_PTZ_VELOCITY_CONTROL) &&
          (cfg->mode != PLAYER_PTZ_POSITION_CONTROL))
      {
        PLAYER_WARN1("unknown control mode requested: %d", cfg->mode);
        Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_PTZ_REQ_CONTROL_MODE);

        return 0;
      }
      else
      {
        controlmode = cfg->mode;
        Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_PTZ_REQ_CONTROL_MODE);
        return 0;
      }
	}
    
    return -1;
}
 
void 
AmtecPowerCube::Main()
{
  player_ptz_data_t data;

//  short lastpan, lasttilt;
//  short newpan, newtilt;
  short currpan, currtilt;

//  short lastpanspeed, lasttiltspeed;
//  short newpanspeed, newtiltspeed;
  short currpanspeed, currtiltspeed;

  // read the current state
/*  if(GetPanTiltPos(&lastpan,&lasttilt) < 0)
  {
    PLAYER_ERROR("GetPanTiltPos() failed; bailing.");
    pthread_exit(NULL);
  }
  if(GetPanTiltVel(&lastpanspeed,&lasttiltspeed) < 0)
  {
    PLAYER_ERROR("GetPanTiltVel() failed; bailing.");
    pthread_exit(NULL);
  }*/

  for(;;)
  {
    pthread_testcancel();

    ProcessMessages();

    if(GetPanTiltPos(&currpan,&currtilt))
    {
      PLAYER_ERROR("GetPanTiltPos() failed(); bailing.");
      pthread_exit(NULL);
    }
    if(GetPanTiltVel(&currpanspeed,&currtiltspeed))
    {
      PLAYER_ERROR("GetPanTiltVel() failed(); bailing.");
      pthread_exit(NULL);
    }

    data.pan = (currpan);
    data.tilt = (currtilt);
    data.zoom = 0;
    data.panspeed = (currpanspeed);
    data.tiltspeed = (currtiltspeed);

    Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, (unsigned char*)&data, sizeof(player_ptz_data_t),NULL);

    // get the module state (for debugging and warning)
    unsigned int state;
    if(GetUint32Param(AMTEC_MODULE_PAN, AMTEC_PARAM_CUBESTATE, &state) < 0)
    {
      PLAYER_ERROR("GetUint32Param() failed; bailing");
      pthread_exit(NULL);
    }
    //printf("state: 0x%x\n", state);
    if(state & AMTEC_STATE_ERROR)
    {
      PLAYER_ERROR1("the Amtec unit has encountered an error and will need\n"
                    "    to be reset; bailing.   Current module state: 0x%x", 
                    state);
      pthread_exit(NULL);
    }
    
    usleep(AMTEC_SLEEP_TIME_USEC);
  }
}