wbr914.cc

机器人仿真软件

{

  ssize_t numread = 0;
  ssize_t newread = 0;
  bool    readSlipped = false;
  int     i;

//      printf( "len=%d numread=%d\n", len, numread );
  while ( numread < (int)len )
  {
    if ( ( newread = read( this->_fd, s, len )) == -1 )
    {
      if( !_fd_blocking && errno == EAGAIN)
      {
	if ( readSlipped == false )
	  printf( "read() slipped\n" );
	else
	{
	  printf("read() failed: %s\n", strerror(errno));
	  return(-1);
	}

	readSlipped = true;

	usleep( DELAY_US);
	continue;
      }

      printf("read() failed: %s\n", strerror(errno));
      return(-1);
    }
    else
    {
      numread += newread;
      if ( _fd_blocking )
      {
	break;
      }
    }
  }

#ifdef DEBUG
  printf("read: %d of %d bytes - ", numread, len );
  ssize_t i;
  for( i=0; i<numread; i++)
  {
    printf( "0x%02x ", s[i] );
  }
  printf( "\n" );
#endif

  uint8_t chksum = 0;

  // Verify the checksum is correct
  for ( i=0; i<numread; i++ )
  {
    chksum += s[i];
  }
  if ( chksum != 0 )
  {
    printf( "Read checksum error\n" );
  }

  // PMD 3410 error code in first byte
  if ( s[0] != 0 && s[0] != 1 )
  {
    const char* err = GetPMDErrorString( s[0] );
    printf( "Cmd error: %s\n", err );
    return( -s[0] );
  }

  return( numread );
}

int wbr914::WriteBuf(unsigned char* s, size_t len)
{
  size_t numwritten;
  int thisnumwritten;
  numwritten=0;

#ifdef DEBUG
  printf("write: %d bytes - ", len );
  size_t i;
  for( i=0; i<len; i++)
  {
    printf( "0x%02x ", s[i] );
  }
  printf( "\n" );
#endif

  while(numwritten < len)
  {
    if((thisnumwritten = write(this->_fd,s+numwritten,len-numwritten)) < 0)
    {
      if(!this->_fd_blocking && errno == EAGAIN)
      {
	printf( "write() slipped\n" );
        usleep( DELAY_US);
        continue;
      }
      printf("write() failed: %s\n", strerror(errno));
      return(-1);
    }
    numwritten += thisnumwritten;
  }
  /*
  ioctl( this->_fd, TIOCMSET, _tc_num );
  if( _tc_num[0] == 2 )
  {
    _tc_num[0] = 0;
  }
  else
  {
    _tc_num[0] = 2;
  }
  */

  return numwritten;
}

int wbr914::sendCmdCom( unsigned char address, unsigned char c, 
			int cmd_num, unsigned char* arg, 
			int ret_num, unsigned char * ret )
{
  assert( cmd_num<=4 );

  unsigned char cmd[8];
  //bool retry = true;
  unsigned char savecs;
  
  cmd[0] = address;
  cmd[1] = 0;          // checksum. to be overwritten
  cmd[2] = 0x00;
  cmd[3] = c;

  // Add arguments to command
  int i;
  for ( i=0; i<cmd_num; i++ )
  {
    cmd[4+i] = arg[i];
  }

  // compute checksum. Ignore cmd[1] since it is the checksum location
  int chk = 0;
  for ( i=0; i<cmd_num+4; i++ )
  {
    chk += cmd[i];
  }

  // Set the checksum
  int cs = -chk;
  cmd[1] = (unsigned char) (cs & 0xff);
  savecs = cmd[1];

  int result;

  // Flush the receive buffer. It should be empty so lets make it be so.
  tcflush( _fd, TCIFLUSH );

  result = WriteBuf( cmd, 4+cmd_num );

  if( result != 4+cmd_num )
  {
    printf( "failed to send command %d\n", (int)c );
    return( -1 );
  }


  if( ret != NULL && ret_num > 0 )
  {
    //    if ( _fd_blocking == false )
      usleep( DELAY_US );

    int rc; 
    if( (rc = ReadBuf( ret, ret_num )) < 0 )
    {
      //      printf( "failed to read response\n" );
    }
    //    else
  }

//      PLAYER_WARN1( "cmd: 0x%4x", *((int *) cmd) );
//      PLAYER_WARN1( "cmd: 0x%4x", *((int *) &(cmd[4])) );
  return result;
}

int wbr914::sendCmd0( unsigned char address, unsigned char c, 
		      int ret_num, unsigned char * ret )
{
  return sendCmdCom( address, c, 0, NULL, ret_num, ret );
}

int wbr914::sendCmd16( unsigned char address, unsigned char c, 
		       int16_t arg, int ret_num, unsigned char * ret )
{

  unsigned char args[2];
  uint16_t a = htons( arg );

  args[1] = (a >> 8) & 0xFF;
  args[0] = (a >> 0) & 0xFF;

  return sendCmdCom( address, c, 2, args, ret_num, ret );
}

int wbr914::sendCmd32( unsigned char address, unsigned char c, 
		       int32_t arg, int ret_num, unsigned char * ret )
{
  unsigned char args[4];
  uint32_t a = htonl( arg );
  args[3] = (a >> 24) & 0xFF;
  args[2] = (a >> 16) & 0xFF;
  args[1] = (a >> 8 ) & 0xFF;
  args[0] = (a >> 0 ) & 0xFF;

  return sendCmdCom( address, c, 4, args, ret_num, ret );
}


void wbr914::SetOdometry( player_position2d_set_odom_req_t* od )
{
  unsigned char ret[2];

  x = od->pose.px;
  y = od->pose.py;
  yaw = od->pose.pa;

  int32_t leftPos = Meters2Ticks( x );
  int32_t rightPos = Meters2Ticks( y );

  int rc;

  rc = sendCmd32( LEFT_MOTOR, SETACTUALPOS, leftPos, 2, ret ); 
  rc = sendCmd32( LEFT_MOTOR, SETACTUALPOS, rightPos, 2, ret ); 
}

int wbr914::GetAnalogSensor(int s, short * val )
{
  unsigned char ret[6];

  sendCmd16( s / 8, READANALOG, s % 8, 6, ret );

  // Analog sensor values are 10 bit values that have been left shifted
  // 6 bits, so right-shift them back
  uint16_t v = ( (uint16_t)BytesToInt16(  &(ret[2]) ) >> 6) & 0x03ff;

  if ( _debug )
    printf( "sensor %d value: 0x%hx\n", s, v );

  *val = (uint16_t)v;

  return 0;
}


void wbr914::GetDigitalIn( uint16_t* d )
{
  unsigned char ret[6];

  sendCmd16( 0, READDIGITAL, 0, 6, ret );

  *d = (uint16_t)BytesToInt16(  &(ret[2]) );
}

void wbr914::SetDigitalOut( uint16_t d )
{
  unsigned char ret[2];

  sendCmd32( 0, WRITEDIGITAL, d, 2, ret );
}

/*
  Robot commands
 */

void wbr914::UpdateM3()
{
  unsigned char ret[2];

  sendCmd0( LEFT_MOTOR,  UPDATE, 2, ret );
  sendCmd0( RIGHT_MOTOR, UPDATE, 2, ret );
}

void wbr914::SetVelocity( uint8_t chan, float mps )
{
  uint8_t ret[2];
  int flip = 1;
  if ( chan == LEFT_MOTOR )
  {
    flip = -1;
  }
  sendCmd32( chan, SETVEL, MPS2Vel( mps )*flip, 2, ret );
}

void wbr914::SetVelocity( float mpsL, float mpsR )
{
  uint8_t ret[2];

  sendCmd32( LEFT_MOTOR,  SETVEL, -MPS2Vel( mpsL ), 2, ret );
  sendCmd32( RIGHT_MOTOR, SETVEL, MPS2Vel( mpsR ), 2, ret );
}

void wbr914::SetVelocityInTicks( int32_t left, int32_t right )
{
  uint8_t ret[2];
  sendCmd32( LEFT_MOTOR,  SETVEL, -left, 2, ret );
  sendCmd32( RIGHT_MOTOR, SETVEL, right, 2, ret );
}

void wbr914::Move( uint8_t chan, float meters )
{
  uint8_t ret[6];
  int flip = 1;

  if ( chan == LEFT_MOTOR )
  {
    flip = -1;
  }

  sendCmd0( chan, GETACTUALPOS, 6, ret );
  int32_t loc = BytesToInt32( &ret[2] );
  loc += (flip*Meters2Ticks( meters ));
  sendCmd32( chan, SETPOS, loc, 2, ret );
}

void wbr914::Move( float metersL, float metersR )
{
  Move( LEFT_MOTOR,   metersL );
  Move( RIGHT_MOTOR,  metersR );
}

void wbr914::SetPosition( uint8_t chan, float meters )
{
  uint8_t ret[6];
  int flip = 1;
  if ( chan == LEFT_MOTOR )
  {
    flip = -1;
  }

  sendCmd32( chan, SETPOS, flip*Meters2Ticks( meters ), 2, ret );
}

void wbr914::SetPosition( float metersL, float metersR )
{
  SetPosition( LEFT_MOTOR,   metersL );
  SetPosition( RIGHT_MOTOR,  metersR );
}

void wbr914::SetActualPositionInTicks( int32_t left, int32_t right )
{
  uint8_t ret[6];
  sendCmd32( LEFT_MOTOR, SETACTUALPOS, -left, 6, ret );
  sendCmd32( RIGHT_MOTOR, SETACTUALPOS, right, 6, ret );
}

void wbr914::SetActualPosition( float metersL, float metersR )
{
  uint8_t ret[6];
  sendCmd32( LEFT_MOTOR, SETACTUALPOS, -Meters2Ticks( metersL ), 6, ret );
  sendCmd32( RIGHT_MOTOR, SETACTUALPOS, Meters2Ticks( metersL ), 6, ret );
}

void wbr914::GetPositionInTicks( int32_t* left, int32_t* right )
{
  uint8_t ret[6];
  sendCmd0( LEFT_MOTOR, GETACTUALPOS, 6, ret );
  *left = -BytesToInt32( &ret[2] );
  sendCmd0( RIGHT_MOTOR, GETACTUALPOS, 6, ret );
  *right = BytesToInt32( &ret[2] );
}

void wbr914::GetVelocityInTicks( int32_t* left, int32_t* right )
{
  uint8_t ret[6];
  sendCmd0( LEFT_MOTOR, GETACTUALVEL, 6, ret );
  *left = -BytesToInt32( &ret[2] );
  sendCmd0( RIGHT_MOTOR, GETACTUALVEL, 6, ret );
  *right = BytesToInt32( &ret[2] );
}

void wbr914::SetAccelerationProfile()
{
  uint8_t ret[2];
  int32_t accel = (int32_t)MOTOR_TICKS_PER_STEP*2;

  // Decelerate faster than accelerating.
  sendCmd32( LEFT_MOTOR,  SETACCEL, accel, ACCELERATION_DEFAULT, ret );
  sendCmd32( RIGHT_MOTOR, SETACCEL, accel, ACCELERATION_DEFAULT, ret );
  sendCmd32( LEFT_MOTOR,  SETDECEL, accel, DECELERATION_DEFAULT, ret );
  sendCmd32( RIGHT_MOTOR, SETDECEL, accel, DECELERATION_DEFAULT, ret );
  SetContourMode( TrapezoidalProfile );
}

void wbr914::Stop( int StopMode ) {
        
  unsigned char ret[8];
  
  if ( _debug )
    printf( "Stop\n" );

  /* Start with motor 0*/
  _stopped = true;
  
  if( StopMode == FULL_STOP )
  {
    sendCmd16( LEFT_MOTOR, RESETEVENTSTATUS, 0x0000, 2, ret );
    sendCmd16( LEFT_MOTOR, SETSTOPMODE, AbruptStopMode, 2, ret );
    sendCmd32( LEFT_MOTOR, SETVEL, 0, 2, ret );
    sendCmd32( LEFT_MOTOR, SETACCEL, 0, ACCELERATION_DEFAULT, ret );
    sendCmd32( LEFT_MOTOR, SETDECEL, 0, DECELERATION_DEFAULT, ret );

    sendCmd16( RIGHT_MOTOR, RESETEVENTSTATUS, 0x0000, 2, ret );
    sendCmd16( RIGHT_MOTOR, SETSTOPMODE, AbruptStopMode, 2, ret );
    sendCmd32( RIGHT_MOTOR, SETVEL, 0, 2, ret );
    sendCmd32( RIGHT_MOTOR, SETACCEL, 0, ACCELERATION_DEFAULT, ret );
    sendCmd32( RIGHT_MOTOR, SETDECEL, 0, DECELERATION_DEFAULT, ret );

    SetContourMode( VelocityContouringProfile );

    EnableMotors( false );
    UpdateM3();

    sendCmd0( LEFT_MOTOR, RESET, 2, ret );
    sendCmd0( RIGHT_MOTOR, RESET, 2, ret );

  }
  else
  {
    sendCmd16( LEFT_MOTOR, RESETEVENTSTATUS, 0x0700, 2, ret );
    sendCmd32( LEFT_MOTOR, SETVEL, 0, 2, ret );

    sendCmd16( RIGHT_MOTOR, RESETEVENTSTATUS, 0x0700, 2, ret );
    sendCmd32( RIGHT_MOTOR, SETVEL, 0, 2, ret );

    UpdateM3();

    sendCmd0( LEFT_MOTOR, RESET, 2, ret );
    sendCmd0( RIGHT_MOTOR, RESET, 2, ret );
  }
}


void wbr914::StopRobot()
{
  Stop( FULL_STOP );
}


bool wbr914::EnableMotors( bool enable )
{
  unsigned char ret[2];
  long torque = 0;
  int rc;

  if ( enable )
  {
    torque = _percentTorque*0x8000/100;
    if ( torque > 0x8000 )
      torque = 0x8000;
  }

  // Need to turn off motors to change the torque
  rc = sendCmd16( LEFT_MOTOR,  SETMOTORMODE, 0, 2, ret );
  rc = sendCmd16( RIGHT_MOTOR, SETMOTORMODE, 0, 2, ret );

  rc = sendCmd16( LEFT_MOTOR, SETMOTORCMD, (short)torque, 2, ret );
  rc = sendCmd16( RIGHT_MOTOR, SETMOTORCMD, (short)torque, 2, ret );

  // Update the torque setting
  UpdateM3();

  if ( enable )
  {
    rc = sendCmd16( LEFT_MOTOR,  SETMOTORMODE, 1, 2, ret );
    rc = sendCmd16( RIGHT_MOTOR, SETMOTORMODE, 1, 2, ret );
  }

  _motorsEnabled = enable;

  return ( true );
}

void wbr914::SetContourMode( ProfileMode_t prof )
{
  uint8_t ret[2];

  sendCmd16( LEFT_MOTOR, SETPROFILEMODE, prof, 2, ret);
  sendCmd16( RIGHT_MOTOR, SETPROFILEMODE, prof, 2, ret);
}


void wbr914::SetMicrosteps()
{
  uint8_t ret[2];

  sendCmd16( LEFT_MOTOR, SETPHASECOUNTS, (short)MOTOR_TICKS_PER_STEP*4, 2, ret);
  sendCmd16( RIGHT_MOTOR, SETPHASECOUNTS, (short)MOTOR_TICKS_PER_STEP*4, 2, ret);
}


int32_t wbr914::Meters2Ticks( float meters )
{
  return (int32_t)( (double)meters * GEAR_RATIO * MOTOR_TICKS_PER_REV / WHEEL_CIRC);
}

float wbr914::Ticks2Meters( int32_t ticks )
{
  return (float)( WHEEL_CIRC*((double)ticks/GEAR_RATIO)/ MOTOR_TICKS_PER_REV );
}

int32_t wbr914::MPS2Vel( float mps )
{
  return (int32_t)( (double)mps * _velocityK );

}

float wbr914::Vel2MPS( int32_t count )
{
  return (float)( (double)count/_velocityK );

}

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

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

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

  //this could just be ntohl

  return data;
}

int16_t wbr914::BytesToInt16(unsigned char *ptr)
{
  unsigned char char0,char1;
  int16_t data = 0;

  char0 = ptr[1];
  char1 = ptr[0];

  data |=  ((int)char0)        & 0x000000FF;
  data |= (((int)char1) << 8)  & 0x0000FF00;

  //this could just be ntohl

  return data;
}