reb.cc

机器人仿真平台,和stage配合运行

  d->position.ypos = htonl(ry);
  d->position.yaw = htons( rtheta);
  d->position.xspeed = htons(rv);
  d->position.yawspeed = htons( rtd);
  //  d->position.heading = htons(this->desired_heading);
  d->position.stall = target_status;

  // later we read the torques FIX

  // update last time
  this->last_position = curr;
}
  


/* this will reset odometry -- the counters on the encoders and 
 * the integrated position estimates
 *
 * returns:
 */
/*void
REB::ResetOdometry()
{

    SetPosCounter(REB_MOTOR_LEFT, 0);
  SetPosCounter(REB_MOTOR_RIGHT, 0);

  this->data->position.x = 0;
  this->data->position.y = 0;
  this->data->position.theta = 0;
  
}
*/

/* this will set the odometry to a given position
 * ****NOTE: assumes that the arguments are in network byte order!*****
 *
 * returns: 
 */
void
REB::SetOdometry(int x, int y, short theta)
{

  SetPosCounter(REB_MOTOR_LEFT, 0);
  SetPosCounter(REB_MOTOR_RIGHT, 0);

  // we assume these are already in network byte order!!!!
  this->data->position.xpos = x;
  this->data->position.ypos = y;
  this->data->position.yaw = theta;

}

/* this will write len bytes from buf out to the serial port reb_fd 
 *   
 *  returns: the number of bytes written, -1 on error
 */
int
REB::write_serial(char *buf, int len)
{
  int num, t,i, pret;
  
#ifdef DEBUG_SERIAL
  printf("WRITE: len=%d: ", len);
  for (i =0; i < len; i++) {
    if (isspace(buf[i])) {
      if (buf[i] == ' ') {
	printf("%c", ' ');
      } else {
	printf("'%02x'", buf[i]);
      }
    } else {
      printf("%c", buf[i]);
    }
  }
  printf("\n");
#endif
  
  num = 0;
  while (num < len) {

    // wait for channel so we can write
    pret = poll(&write_pfd, 1, 1000);
    
    if (pret < 0) {
      fprintf(stderr, "REB: write_serial: poll returned error\n");
      perror("write_serial");
      exit(-1);
    } else if (pret == 0) {
      fprintf(stderr, "REB: write_serial: poll timed out!\n");
      return -1;
    }

    t = write(this->reb_fd, buf+num, len-num);
    if (t < 0) {
      switch(errno) {
      case EBADF:
	fprintf(stderr, "bad file descriptor!!\n");
	break;
      }

      fprintf(stderr, "error writing\n");
      for (i = 0; i < len; i++) {
	fprintf(stderr, "%c (%02x) ", isprint(buf[i]) ? buf[i] : ' ',
	       buf[i]);
      }
      fprintf(stderr, "\n");
      return -1;
    }
    
#ifdef _DEBUG
    printf("WRITES: wrote %d of %d\n", t, len);
#endif
    
    num += t;
  }
  return len;
}

/* this will read upto len bytes from reb_fd or until it reads the
   flag string given by flag (length is flen)

   returns: number of bytes read
*/
int
REB::read_serial_until(char *buf, int len, char *flag, int flen)
{
  int num=0,t, pret;
  
#ifdef DEBUG_SERIAL
  printf("RSU before while flag len=%d len=%d\n", flen, len);
#endif
  
  while (num < len-1) {

    // wait for channel to have data first...
    pret = poll(&read_pfd, 1, 2000);
    
    if (pret < 0) {
      perror("read_serial_until");
      exit(-1);
    } else if (pret == 0) {
      fprintf(stderr, "REB: read_serial_until timed out!\n");
      return -1;
    }


    // now we can read
    t = read(this->reb_fd, buf+num, 1);

#ifdef DEBUG_SERIAL
    printf("RSU: %c (%02x)\n", isspace(buf[num]) ? ' ' : buf[num], buf[num]);
#endif
    
    if (t < 0) {
      fprintf(stderr, "error!!!\n");
      switch(errno) {
      case EAGAIN:
	t = 0;
	break;
      case EINTR:
	return -1;
      default:
	return -1;
      }
    }
    
    num ++;
    buf[num] = '\0';
    if (num >= flen) {
      
      if (!strcmp(buf+num-flen, flag)) {
	return 0;
      }
    }
    
    if (num>= 2) {
      buf[num] = '\0';
      if (strcmp(buf+num-2, "\r\n") == 0) {
	num =0;
#ifdef DEBUG_SERIAL
	printf("RSU: MATCHED CRLF\n");
#endif
      }
    }
  }
  
  buf[num] = '\0';
  return num;
}


/* this will take the given buffer, which should have a command in it,
 * and write it to the serial port, then read a response back into the
 * buffer
 *
 * returns: number of bytes read
 */
int
REB::write_command(char *buf, int len, int maxsize)
{
  static int total=0;
  int ret;
  char rbuf[256];
  int rcount=0;
  assert(maxsize < 256);

  while (1) {
    ret = read_serial_until(rbuf, 256, REB_COMMAND_PROMPT, strlen(REB_COMMAND_PROMPT));

    total += write_serial(buf, len);

    do {
      rcount=0;
      ret = read_serial_until(rbuf, maxsize, CRLF, strlen(CRLF));
    } while (rbuf[0] != tolower(buf[0]) && rcount++ < 2 && ret >= 0);

    if (ret < 0) {
      Restart();
      continue;
    }

    total += ret;
    break;
  }

  memcpy(buf, rbuf, maxsize);
  return ret;
}

/* this sends the restart command to the Kam
 *
 * returns: 
 */
void
REB::Restart()
{
  char buf;
  int ret=0,pret=0;
  
  struct pollfd pfd;

  pfd.fd = this->reb_fd;
  pfd.events = POLLIN;

  printf("REB: flushing read channel: ");
  fflush(stdout);
  do {
    pret = poll(&pfd, 1, 2000);

    if (pret) {
      ret = read(this->reb_fd, &buf, 1);
      if (ret) {
	if (isalnum(buf)) {
	  printf("%c", buf);
	} else {
	  printf("%02x", buf);
	}
      }
    } else {
      printf("timed out");
      break;
    }
  } while (ret);
  printf("\n");
  

  // restart the control software on the REB
  printf("REB: sending restart...");

  fflush(stdout);
  write_serial(REB_RESTART_COMMAND, strlen(REB_RESTART_COMMAND));

  printf("done\n");
}

/* sets the state of the IR. REB_IR_START (true) turns
 * them on, REB_IR_STOP (False) turns em off
 *
 * returns: 
 */
void
REB::SetIRState(int action)
{
  char buf[64];
 
  sprintf(buf, "Y,%c\r", action ? '1' : '0');

  write_command(buf, strlen(buf), sizeof(buf));
}

/* this will configure the AD channel given.
 *  0 == channel OFF
 *  1 == channel ON
 *  2 == toggle channel state
 */
void
REB::ConfigAD(int channel, int action)
{
  char buf[64];

  sprintf(buf, "Q,%d,%c\r", channel, '0'+action);

  write_command(buf, strlen(buf), sizeof(buf));
}

/* this will read the given AD channel
 *
 * returns: the value of the AD channel
 */
unsigned short
REB::ReadAD(int channel) 
{
  char buf[64];

  sprintf(buf, "I,%d\r", channel);
  write_command(buf, strlen(buf), sizeof(buf));
  
  return atoi(&buf[2]);
}

/* reads all the IR values at once.  stores them
 * in the uint16_t array given as arg ir
 *
 * returns: 
 */
void
REB::ReadAllIR(uint16_t *ir)
{
  char buf[64];
  int ret;

  sprintf(buf, "W\r");
  ret = write_command(buf, strlen(buf), sizeof(buf));
    
  size_t p=0;
  for (int i =0; i < PLAYER_IR_MAX_SAMPLES; i++) {
    // find the first comma
    while (buf[p++] != ',') {
      if (p >= strlen(buf)) {
	return;
      }
    }
    ir[i] = atoi(&buf[p]);
  }
}

/* this will set the desired speed for the given motor mn
 *
 * returns:
 */
void
REB::SetSpeed(int mn, int speed)
{
  char buf[64];
  
  sprintf(buf, "D,%c,%d\r", '0'+mn, speed);

  write_command(buf, strlen(buf), sizeof(buf));
}

/* reads the current speed of motor mn
 *
 * returns: the speed of mn
 */
int
REB::ReadSpeed(int mn)
{
  char buf[64];
  
  sprintf(buf, "E,%c\r", '0'+mn);

  write_command(buf, strlen(buf), sizeof(buf));

  return atoi(&buf[2]);
}

/* this sets the desired position motor mn should go to
 *
 * returns:
 */
void
REB::SetPos(int mn, int pos)
{
  char buf[64];
  
  sprintf(buf,"C,%c,%d\r", '0'+mn,pos);

  write_command(buf, strlen(buf), sizeof(buf));
}

/* this sets the position counter of motor mn to the given value
 *
 * returns:
 */
void
REB::SetPosCounter(int mn, int pos)
{
  char buf[64];
 
  sprintf(buf,"G,%c,%d\r", '0'+mn,pos);
  write_command(buf, strlen(buf), sizeof(buf));
}

/* this will read the current value of the position counter
 * for motor mn
 *
 * returns: the current position for mn
 */
int
REB::ReadPos(int mn)
{
  char buf[64];
  
  sprintf(buf, "H,%c\r", '0'+mn);
  write_command(buf, strlen(buf), sizeof(buf));
  
  return atoi(&buf[2]);
}

/* this will configure the position PID for motor mn
 * using paramters Kp, Ki, and Kd
 *
 * returns:
 */
void
REB::ConfigPosPID(int mn, int kp, int ki, int kd)
{ 
  char buf[64];

  sprintf(buf, "F,%c,%d,%d,%d\r", '0'+mn, kp,ki,kd);
  write_command(buf, strlen(buf), sizeof(buf));
}

/* this will configure the speed PID for motor mn
 *
 * returns:
 */
void
REB::ConfigSpeedPID(int mn, int kp, int ki, int kd)
{
  char buf[64];
  
  sprintf(buf, "A,%c,%d,%d,%d\r", '0'+mn, kp,ki,kd);
  
  write_command(buf, strlen(buf), sizeof(buf));
}

/* this will set the speed profile for motor mn
 * it takes the max velocity and acceleration
 *
 * returns:
 */
void
REB::ConfigSpeedProfile(int mn, int speed, int acc)
{
  char buf[64];
  
  sprintf(buf, "J,%c,%d,%d\r", '0'+mn, speed,acc);
  write_command(buf, strlen(buf), sizeof(buf));
}

/* this will read the status of the motion controller.
 * mode is set to 1 if in position mode, 0 if velocity mode
 * error is set to the position/speed error
 *
 * returns: target status: 1 is on target, 0 is not on target
 */
unsigned char
REB::ReadStatus(int mn, int *mode, int *error)
{
  char buf[64];

  sprintf(buf, "K,%c\r", '0'+mn);
  write_command(buf, strlen(buf), sizeof(buf));

  // buf now has the form "k,target,mode,error"
  int target;

  sscanf(buf, "k,%d,%d,%d", &target, mode, error);

  return (unsigned char)target;
}