obot.cc

机器人仿真软件

int
Obot::WriteBuf(unsigned char* s, size_t len)
{
  size_t numwritten;
  int thisnumwritten;
  unsigned char ack[1];

  /*
  static double last = 0.0;
  double t;
  GlobalTime->GetTimeDouble(&t);
  printf("WriteBuf: %d bytes (time since last: %f)\n", 
         len, t-last);
  last=t;
  */

  for(;;)
  {
    numwritten=0;
    while(numwritten < len)
    {
      if((thisnumwritten = write(this->fd,s+numwritten,len-numwritten)) < 0)
      {
        if(!this->fd_blocking && errno == EAGAIN)
        {
          usleep(OBOT_DELAY_US);
          continue;
        }
        PLAYER_ERROR1("write() failed: %s", strerror(errno));
        return(-1);
      }
      numwritten += thisnumwritten;
    }

    // get acknowledgement
    if(ReadBuf(ack,1) < 0)
    {
      PLAYER_ERROR("failed to get acknowledgement");
      return(-1);
    }

    // TODO: re-init robot on NACK, to deal with underlying cerebellum reset 
    // problem
    switch(ack[0])
    {
      case OBOT_ACK:
        usleep(OBOT_DELAY_US);
        return(0);
      case OBOT_NACK:
        PLAYER_WARN("got NACK; reinitializing connection");
        usleep(OBOT_DELAY_US);
        if(close(this->fd) < 0)
          PLAYER_WARN1("close failed: %s", strerror(errno));
        if(OpenTerm() < 0)
        {
          PLAYER_ERROR("failed to re-open connection");
          return(-1);
        }
        if(InitRobot() < 0)
        {
          PLAYER_ERROR("failed to reinitialize");
          return(-1);
        }
        else
        {
          usleep(OBOT_DELAY_US);
          return(0);
        }
        break;
      default:
        PLAYER_WARN1("got unknown value for acknowledgement: %d",ack[0]);
        usleep(OBOT_DELAY_US);
        return(-1);
    }
  }
}

int 
Obot::BytesToInt32(unsigned char *ptr)
{
  unsigned char char0,char1,char2,char3;
  int data = 0;

  char0 = ptr[0];
  char1 = ptr[1];
  char2 = ptr[2];
  char3 = ptr[3];

  data |=  ((int)char0)        & 0x000000FF;
  data |= (((int)char1) << 8)  & 0x0000FF00;
  data |= (((int)char2) << 16) & 0x00FF0000;
  data |= (((int)char3) << 24) & 0xFF000000;

  return data;
}

int
Obot::GetBatteryVoltage(int* voltage)
{
  unsigned char buf[5];
  buf[0] = OBOT_GET_VOLTAGE;

  if(WriteBuf(buf,1) < 0)
  {
    PLAYER_ERROR("failed to send battery voltage command");
    return(-1);
  }

  if(ReadBuf(buf,5) < 0)
  {
    PLAYER_ERROR("failed to read battery voltage");
    return(-1);
  }

  if(ValidateChecksum(buf,5) < 0)
  {
    PLAYER_ERROR("checksum failed on battery voltage");
    return(-1);
  }

  *voltage = BytesToInt32(buf);
  return(0);
}

void
Obot::Int32ToBytes(unsigned char* buf, int i)
{
  buf[0] = (i >> 0)  & 0xFF;
  buf[1] = (i >> 8)  & 0xFF;
  buf[2] = (i >> 16) & 0xFF;
  buf[3] = (i >> 24) & 0xFF;
}

int
Obot::GetOdom(int *ltics, int *rtics, int *lvel, int *rvel)
{
  unsigned char buf[20];
  int index;

  buf[0] = OBOT_GET_ODOM;
  if(WriteBuf(buf,1) < 0)
  {
    PLAYER_ERROR("failed to send command to retrieve odometry");
    return(-1);
  }
  //usleep(OBOT_DELAY_US);
  
  // read 4 int32's, 1 error byte, and 1 checksum
  if(ReadBuf(buf, 18) < 0)
  {
    PLAYER_ERROR("failed to read odometry");
    return(-1);
  }

  if(ValidateChecksum(buf, 18) < 0)
  {
    PLAYER_ERROR("checksum failed on odometry packet");
    return(-1);
  }
  if(buf[16] == 1)
  {
    PLAYER_ERROR("Cerebellum error with encoder board");
    return(-1);
  }

  index = 0;
  *ltics = BytesToInt32(buf+index);
  index += 4;
  *rtics = BytesToInt32(buf+index);
  index += 4;
  *rvel = BytesToInt32(buf+index);
  index += 4;
  *lvel = BytesToInt32(buf+index);

  //printf("ltics: %d rtics: %d\n", *ltics, *rtics);

  //puts("got good odom packet");

  return(0);
}

int 
Obot::ComputeTickDiff(int from, int to) 
{
  int diff1, diff2;

  // find difference in two directions and pick shortest
  if(to > from) 
  {
    diff1 = to - from;
    diff2 = (-OBOT_MAX_TICS - from) + (to - OBOT_MAX_TICS);
  }
  else 
  {
    diff1 = to - from;
    diff2 = (from - OBOT_MAX_TICS) + (-OBOT_MAX_TICS - to);
  }

  if(abs(diff1) < abs(diff2)) 
    return(diff1);
  else
    return(diff2);
}

void
Obot::UpdateOdom(int ltics, int rtics)
{
  int ltics_delta, rtics_delta;
  double l_delta, r_delta, a_delta, d_delta;
  int max_tics;
  static struct timeval lasttime;
  struct timeval currtime;
  double timediff;

  if(this->motors_swapped)
  {
    int tmp = ltics;
    ltics = rtics;
    rtics = tmp;
  }

  if(!this->odom_initialized)
  {
    this->last_ltics = ltics;
    this->last_rtics = rtics;
    gettimeofday(&lasttime,NULL);
    this->odom_initialized = true;
    return;
  }
  
  // MAJOR HACK! 
  // The problem comes from one or the other encoder returning 0 ticks (always
  // the left, I think), we'll just throw out those readings.  Shouldn't have
  // too much impact.
  if(!ltics || !rtics)
  {
    PLAYER_WARN("Invalid odometry reading (zeros); ignoring");
    return;
  }

  //ltics_delta = ComputeTickDiff(last_ltics,ltics);
  //rtics_delta = ComputeTickDiff(last_rtics,rtics);
  ltics_delta = ltics - this->last_ltics;
  rtics_delta = rtics - this->last_rtics;

  // mysterious rollover code borrowed from CARMEN
/*
  if(ltics_delta > SHRT_MAX/2)
    ltics_delta += SHRT_MIN;
  if(ltics_delta < -SHRT_MIN/2)
    ltics_delta -= SHRT_MIN;
  if(rtics_delta > SHRT_MAX/2)
    rtics_delta += SHRT_MIN;
  if(rtics_delta < -SHRT_MIN/2)
    rtics_delta -= SHRT_MIN;
*/

  gettimeofday(&currtime,NULL);
  timediff = (currtime.tv_sec + currtime.tv_usec/1e6)-
             (lasttime.tv_sec + lasttime.tv_usec/1e6);
  max_tics = (int)rint(OBOT_MAX_WHEELSPEED / OBOT_M_PER_TICK / timediff);
  lasttime = currtime;

  //printf("ltics: %d\trtics: %d\n", ltics,rtics);
  //printf("ldelt: %d\trdelt: %d\n", ltics_delta, rtics_delta);
  //printf("maxtics: %d\n", max_tics);

  if(abs(ltics_delta) > max_tics || abs(rtics_delta) > max_tics)
  {
    PLAYER_WARN("Invalid odometry change (too big); ignoring");
    return;
  }

  l_delta = ltics_delta * OBOT_M_PER_TICK;
  r_delta = rtics_delta * OBOT_M_PER_TICK;

  //printf("Left speed: %f\n", l_delta / timediff);
  //printf("Right speed: %f\n", r_delta / timediff);


  a_delta = (r_delta - l_delta) / OBOT_AXLE_LENGTH;
  d_delta = (l_delta + r_delta) / 2.0;


  this->px += d_delta * cos(this->pa);
  this->py += d_delta * sin(this->pa);
  this->pa += a_delta;
  this->pa = NORMALIZE(this->pa);
  
  //printf("obot: pose: %f,%f,%f\n", this->px,this->py, RTOD(this->pa));

  this->last_ltics = ltics;
  this->last_rtics = rtics;
}

// Validate XOR checksum
int
Obot::ValidateChecksum(unsigned char *ptr, size_t len)
{
  size_t i;
  unsigned char checksum = 0;

  for(i = 0; i < len-1; i++)
    checksum ^= ptr[i];

  if(checksum == ptr[len-1])
    return(0);
  else
    return(-1);
}

// Compute XOR checksum
unsigned char
Obot::ComputeChecksum(unsigned char *ptr, size_t len)
{
  size_t i;
  unsigned char chksum = 0;

  for(i = 0; i < len; i++)
    chksum ^= ptr[i];

  return(chksum);
}

int
Obot::SendCommand(unsigned char cmd, int val1, int val2)
{
  unsigned char buf[10];
  int i;

  //printf("SendCommand: %d %d %d\n", cmd, val1, val2);
  i=0;
  buf[i] = cmd;
  i+=1;
  Int32ToBytes(buf+i,val1);
  i+=4;
  Int32ToBytes(buf+i,val2);
  i+=4;
  buf[i] = ComputeChecksum(buf,i);

  if(WriteBuf(buf,10) < 0)
  {
    PLAYER_ERROR("failed to send command");
    return(-1);
  }

  return(0);
}

int
Obot::SetVelocity(int lvel, int rvel)
{
  int retval;
  
  //printf("SetVelocity: %d %d\n", lvel, rvel);

  if(!this->motors_swapped)
    retval = SendCommand(OBOT_SET_VELOCITIES,lvel,rvel);
  else
    retval = SendCommand(OBOT_SET_VELOCITIES,rvel,lvel);
  if(retval < 0)
  {
    PLAYER_ERROR("failed to set velocities");
    return(-1);
  }
  return(0);
}

int 
Obot::ChangeMotorState(int state)
{
  unsigned char buf[1];
  if(state)
    buf[0] = OBOT_ENABLE_VEL_CONTROL;
  else
    buf[0] = OBOT_DISABLE_VEL_CONTROL;
  return(WriteBuf(buf,sizeof(buf)));
}

static void
StopRobot(void* obotdev)
{
  Obot* td = (Obot*)obotdev;

  tcflush(td->fd,TCIOFLUSH);
  if(td->SetVelocity(0,0) < 0)
    PLAYER_ERROR("failed to stop robot on thread exit");
}

// computes the signed minimum difference between the two angles.
double
Obot::angle_diff(double a, double b)
{
  double d1, d2;
  a = NORMALIZE(a);
  b = NORMALIZE(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}