wbr914.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 *
 *   The White Box Robotics Model 914 robot
 *
 */


/** @ingroup drivers */
/** @{ */
/** @defgroup driver_wbr914 wbr914
 * @brief White Box Robotics Model 914 robot

The White Box Robotics Model 914 computer communicates with the M3 I/O and
motor control board over a serial-to-USB driver. The serial commands are
used to communicate with two PMD motion control chips that drive the
stepper motors and control the onboard I/O.

@par Compile-time dependencies

- none

@par Provides

The wbr914 driver provides the following device interfaces, some of
them named:

- @ref interface_position2d
  - This interface returns position data, and accepts velocity commands.
    - PLAYER_POSITION2D_CMD_VEL

- @ref interface_ir
  - This interface returns the IR range data.

- @ref interface_aio
  - This interface returns the analog input data from the optional 2nd I/O board.

- @ref interface_dio
  - This interface returns the digital input information and allows control of the digital outputs on all installed White Box Robotics I/O boards. The first I/O board supplies 8 digital inputs and outputs and the optional second I/O board supplies an additional 8 digital inputs and outputs.


@par Supported configuration requests

- @ref interface_position2d :
  - PLAYER_POSITION_SET_ODOM_REQ
  - PLAYER_POSITION_MOTOR_POWER_REQ
  - PLAYER_POSITION_RESET_ODOM_REQ
  - PLAYER_POSITION_GET_GEOM_REQ

- @ref interface_ir :
  - PLAYER_IR_POSE

@par Supported commands

- @ref interface_position2d :
  - PLAYER_POSITION2D_CMD_VEL

- @ref interface_dio :
  - PLAYER_DIO_CMD_VALUES


@par Configuration file options

- port (string)
  - Default: "/dev/ttyUSB0"

@par Example

@verbatim
driver
(
  name "wbr914"
  provides [ "position2d:0" "ir:0" "aio:0" "dio:0" ]
  port "/dev/ttyUSB0"
)

@endverbatim

@author Ian Gough, White Box Robotics
*/
/** @} */

#ifdef HAVE_CONFIG_H
  #include "config.h"
#endif

#include <unistd.h>
#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */

#include <fcntl.h>
#include <linux/serial.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <termio.h>
#include <termios.h>
#include <sys/socket.h>
#include <netinet/tcp.h>
#include <netdb.h>

#include "wbr914.h"

#undef PLAYER_MSG0
#define PLAYER_MSG0(a,b) LogMe(b)

static void LogMe( const char* s )
{
  FILE* fp = fopen( "plog", "a+" );
  fprintf( fp, s );
  fclose( fp );
}



Driver* wbr914_Init(ConfigFile* cf, int section)
{
  return (Driver*)(new wbr914(cf,section));
}

void wbr914_Register(DriverTable* table)
{
  table->AddDriver("wbr914", wbr914_Init);
}

wbr914::wbr914(ConfigFile* cf, int section)
  : Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN),
    _stopped( true ), _motorsEnabled( false ), _lastDigOut( 0 )
{
  last_lpos = 0;
  last_rpos = 0;

  ErrorInit( 9 );

  // zero ids, so that we'll know later which interfaces were
  // requested
  memset(&this->position_id, 0, sizeof(player_devaddr_t));
  memset(&this->ir_id, 0, sizeof(player_devaddr_t));

  memset(&this->last_position_cmd, 0, sizeof(player_position2d_cmd_vel_t));

  this->position_subscriptions = 0;
  this->ir_subscriptions       = 0;

  // Do we create a robot position interface?
  if(cf->ReadDeviceAddr(&(this->position_id), section, "provides",
                        PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }

  // Do we create an ir interface?
  if(cf->ReadDeviceAddr(&(this->ir_id), section, "provides",
                      PLAYER_IR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->ir_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }

  // Do we create an Analog I/O interface?
  if(cf->ReadDeviceAddr(&(this->aio_id), section, "provides",
                      PLAYER_AIO_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->aio_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }

  // Do we create a Digital I/O interface?
  if(cf->ReadDeviceAddr(&(this->dio_id), section, "provides",
                      PLAYER_DIO_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->dio_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }


  // Read config file options
  _serial_port = cf->ReadString(section,"port",DEFAULT_M3_PORT);
  _percentTorque = cf->ReadInt(section,"torque", DEFAULT_PERCENT_TORQUE );
  _debug = cf->ReadInt(section,"debug",0 );
  _fd = -1;

  // Constrain torque (power to motor phases) between 0 and 100.
  // Smaller numbers mean less torque, but less power used and less
  // heat generated. Not much use reducing the torque setting below
  // 20%.
  if ( _percentTorque > 100 )
  {
    _percentTorque = 100;
  }
  else if ( _percentTorque < 20 )
  {
    _percentTorque = 20;
  }

  // Set up the robot geometry

  // X location in meters
  _robot2d_geom.pose.px     = 0.0;
  // Y location in meters
  _robot2d_geom.pose.py     = 0.0;
  // yaw in rads
  _robot2d_geom.pose.pa     = 0.0;

  // Width in meters
  _robot2d_geom.size.sw     = 0.37;
  // Length in meters
  _robot2d_geom.size.sl     = 0.42;

  _robot3d_geom.pose.px     = _robot2d_geom.pose.px;
  _robot3d_geom.pose.py     = _robot2d_geom.pose.py;
  _robot3d_geom.pose.pz     = 0.0;
  _robot3d_geom.pose.proll  = 0.0;
  _robot3d_geom.pose.ppitch = 0.0;
  _robot3d_geom.pose.pyaw   = _robot2d_geom.pose.pa;

  _robot3d_geom.size.sw     = _robot2d_geom.size.sw;
  _robot3d_geom.size.sl     = _robot2d_geom.size.sl;
  _robot3d_geom.size.sh     = 0.3;

  // Set up the IR array geometry
  _ir_geom.poses_count = NUM_IR_SENSORS;

  _ir_geom.poses[ 0 ].px = 0.030;
  _ir_geom.poses[ 0 ].py = -0.190;
  _ir_geom.poses[ 0 ].pa = DTOR( -90 );

  _ir_geom.poses[ 1 ].px = 0.190;
  _ir_geom.poses[ 1 ].py = -0.090;
  _ir_geom.poses[ 1 ].pa = DTOR( -30 );

  _ir_geom.poses[ 2 ].px = 0.210;
  _ir_geom.poses[ 2 ].py = 0.0;
  _ir_geom.poses[ 2 ].pa = DTOR( 0 );

  _ir_geom.poses[ 3 ].px = 0.190;
  _ir_geom.poses[ 3 ].py = 0.090;
  _ir_geom.poses[ 3 ].pa = DTOR( 30 );

  _ir_geom.poses[ 4 ].px = 0.030;
  _ir_geom.poses[ 4 ].py = 0.190;
  _ir_geom.poses[ 4 ].pa = DTOR( 90 );

  // These 3 sensor have a z value of 0.35m and a pitch of 30 degrees down
  // This type of pose is not currently handled by Player so do the best we
  // can with what we have.
  _ir_geom.poses[ 5 ].px = 0.200;
  _ir_geom.poses[ 5 ].py = -0.060;
  _ir_geom.poses[ 5 ].pa = DTOR( -60 );

  _ir_geom.poses[ 6 ].px = 0.210;
  _ir_geom.poses[ 6 ].py = 0.0;
  _ir_geom.poses[ 6 ].pa = DTOR( 0 );

  _ir_geom.poses[ 7 ].px = 0.200;
  _ir_geom.poses[ 7 ].py = 0.060;
  _ir_geom.poses[ 7 ].pa = DTOR( 60 );
}

/**
   Clean up any resources
 */
wbr914::~wbr914()
{
  Shutdown();
}

int wbr914::Setup()
{
  struct termios term;
  int flags;
  //int ltics,rtics,lvel,rvel;

  printf( "Initializing White Box Robotics Controller on %s...\n", _serial_port);
  fflush(stdout);

  PLAYER_MSG0( 0, "Starting WBR driver\n" );

  // open it.  non-blocking at first, in case there's no robot
  if((this->_fd = open(_serial_port, O_RDWR | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
  {
    printf("open() failed: %s\n", strerror(errno));
    return(-1);
  }

  if(tcgetattr(this->_fd, &term) < 0 )
  {
    printf("tcgetattr() failed: %s\n", strerror(errno));
    close(this->_fd);
    this->_fd = -1;
    return(-1);
  }

  cfmakeraw( &term );
  cfsetispeed( &term, B38400 );
  cfsetospeed( &term, B38400 );

  // 2 stop bits
  term.c_cflag |= CSTOPB | CLOCAL | CREAD;
  term.c_iflag |= IGNPAR;

  if(tcsetattr(this->_fd, TCSADRAIN, &term) < 0 )
  {
    printf("tcsetattr() failed: %s\n", strerror(errno));
    close(this->_fd);
    this->_fd = -1;
    return(-1);
  }

  {
    struct serial_struct serial_info;

    // Custom baud rate of 416666 baud, the max the
    // motor controller will handle.
    // round off to get the closest divisor.
    serial_info.flags = ASYNC_SPD_CUST | ASYNC_LOW_LATENCY;
    serial_info.custom_divisor = (int)((float)24000000.0/(float)416666.0 + 0.5);
    if ( _debug )
      printf( "Custom divisor = %d\n", serial_info.custom_divisor );

    if ( ioctl( _fd, TIOCSSERIAL, &serial_info ) < 0)
    {
      perror("config_serial_port: ioctl TIOCSSERIAL");
      return(-1);
    }
  }

  _fd_blocking = false;

  if ( _debug )
    printf( "InitRobot\n" );
  fflush(stdout);
  if(InitRobot() < 0)
  {
    printf("failed to initialize robot\n");
    close(this->_fd);
    this->_fd = -1;
    return(-1);
  }

  /* ok, we got data, so now set NONBLOCK, and continue */
  if((flags = fcntl(this->_fd, F_GETFL)) < 0)
  {
    printf("fcntl() failed: %s\n", strerror(errno));
    close(this->_fd);
    this->_fd = -1;
    return(-1);
  }

  if(fcntl(this->_fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
  {
    printf("fcntl() failed: %s\n", strerror(errno));
    close(this->_fd);
    this->_fd = -1;
    return(-1);
  }
  _fd_blocking = true;

  _usCycleTime = 154;

  unsigned char ret[4];
  if( sendCmd0( LEFT_MOTOR, GETSAMPLETIME, 4,ret ) < 0)
  {
    printf("failed to get cycle time\n");
    return -1;
  }
  _usCycleTime = BytesToInt16( &(ret[2]) );

  _velocityK = (GEAR_RATIO * MOTOR_TICKS_PER_REV * _usCycleTime * 65536)/(WHEEL_CIRC * 1000000);

  SetMicrosteps();

  // PWM sign magnitude mode
  sendCmd16( LEFT_MOTOR, SETOUTPUTMODE, 1, 2, ret );
  sendCmd16( RIGHT_MOTOR, SETOUTPUTMODE, 1, 2, ret );

  
  /*  This might be a good time to reset the odometry values */
  if ( _debug )
    printf( "ResetRawPositions\n" );
  fflush( stdout );
  ResetRawPositions();

  if ( _debug )
    printf( "SetAccelerationProfile\n" );
  SetAccelerationProfile();
  UpdateM3();

  /* now spawn reading thread */
  if ( _debug )
    printf( "Starting Thread...\n" );
  StartThread();
  return(0);
}

int wbr914::InitRobot()
{

  // initialize the robot
  unsigned char buf[6];
  usleep( DELAY_US);

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

  if ( _debug )
    printf( "GetVersion\n" );
  if( sendCmd0( LEFT_MOTOR, GETVERSION, 6, buf ) < 0)
  {
    printf("failed to initialize robot\n");
    return -1;
  }

  if ( _debug )
    printf( "GetVersion\n" );

  if(sendCmd0( RIGHT_MOTOR, GETVERSION, 6, buf ) < 0)
  {
    printf("failed to initialize robot\n");
    return -1;
  }

  _stopped = true;
  return(0);
}


int wbr914::Shutdown()
{
  if( this->_fd == -1 )
    return(0);

  // Stop any more processing
  StopThread();

  // Stop the robot
  StopRobot();

  EnableMotors( false );

  // Close the connection to the M3
  int fd = _fd;
  this->_fd = -1;
  close( fd );

  puts( "914 has been shut down" );

  return(0);
}

int wbr914::Subscribe( player_devaddr_t id )
{
  // do the subscription
  int rc = Driver::Subscribe(id);

  if( rc == 0)
  {
    // also increment the appropriate subscription counter
    if(Device::MatchDeviceAddress(id, this->position_id))
    {
      this->position_subscriptions++;
    }
    else if(Device::MatchDeviceAddress(id, this->ir_id))
    {
      this->ir_subscriptions++;
    }
    else if(Device::MatchDeviceAddress(id, this->aio_id))
    {
      this->aio_subscriptions++;
    }
    else if(Device::MatchDeviceAddress(id, this->dio_id))
    {
      this->dio_subscriptions++;
    }
  }

  return( rc );
}

int wbr914::Unsubscribe( player_devaddr_t id )
{
  int shutdownResult = Driver::Unsubscribe(id);

  // do the unsubscription
  // and decrement the appropriate subscription counter
  if( shutdownResult == 0 )
  {
    if(Device::MatchDeviceAddress(id, this->position_id))
    {
      this->position_subscriptions--;
      assert(this->position_subscriptions >= 0);
    }
    else if(Device::MatchDeviceAddress(id, this->ir_id))
    {
      this->ir_subscriptions--;
      assert(this->ir_subscriptions >= 0);
    }
    else if(Device::MatchDeviceAddress(id, this->aio_id))
    {
      this->aio_subscriptions--;
      assert(this->aio_subscriptions >= 0);
    }
    else if(Device::MatchDeviceAddress(id, this->dio_id))
    {
      this->dio_subscriptions--;
      assert(this->dio_subscriptions >= 0);
    }
  }

  return(shutdownResult);
}

void wbr914::PublishData(void)
{
  // TODO: something smarter about timestamping.

  if ( position_subscriptions )
  {
    // put odometry data
    this->Publish(this->position_id, NULL,
		  PLAYER_MSGTYPE_DATA,
		  PLAYER_POSITION2D_DATA_STATE,
		  (void*)&(this->_data.position),