er.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000-2003
 *
 *		David Feil-Seifer
 *
 *
 *  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
 *
 */

/*
 *
 * Driver for the "ER" robots, made by Evolution Robotics.    
 *
 * Some of this code is borrowed and/or adapted from the player
 * module of trogdor; thanks to the author of that module.
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_er1 er1
 * @brief Evolution ER1 mobile robot

@todo This driver is currently disabled because it needs to be updated to
the Player 2.0 API.

The er1 driver provides position control of the Evolution Robotics'
ER1 and ERSDK robots.

This driver is new and not thoroughly tested.  The odometry cannot be
trusted to give accurate readings.

You will need a kernel driver to allow the serial port to be seen.
This driver, and news about the player driver can be found <a
href="http://www-robotics.usc.edu/~dfseifer/project-erplayer.php">here</a>.

@todo Implement IR and power interfaces.

NOT DOING: I don't have a gripper, if someone has code for a gripper,
by all means contribute it.  It would be welcome to the mix.

@par Compile-time dependencies

- &lt;asm/ioctls.h&gt;

@par Provides

- @ref interface_position2d

@par Requires

- none

@par Supported configuration requests

- PLAYER_POSITION_GET_GEOM_REQ
- PLAYER_POSITION_MOTOR_POWER_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/usb/ttyUSB0"
  - Serial port used to communicate with the robot.
- axle (length)
  - Default: 0.38 m
  - The distance between the motorized wheels
- motor_dir (integer)
  - Default: 1
  - Direction of the motors; should be 1 or -1.  If the left motor is
    plugged in to the motor 1 port on the RCM, put -1 here instead
- debug (integer)
  - Default: 0
  - Should the driver print out debug messages?
  
@par Example 

@verbatim
driver
(
  name "er1"
  provides ["position2d:0"]
)
@endverbatim

@author David Feil-Seifer
*/
/** @} */


#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include "er.h"
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <netinet/in.h>  /* for struct sockaddr_in, htons(3) */
#include <math.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/time.h>

#include <libplayercore/playercore.h>

static void StopRobot(void* erdev);


// initialization function
Driver* ER_Init( ConfigFile* cf, int section)
{
	return((Driver*)(new ER(cf, section)));
}


// a driver registration function
void 
ER_Register(DriverTable* table)
{
  table->AddDriver("er1", ER_Init);
}

ER::ER(ConfigFile* cf, int section) :
  Driver(cf,section)
{


  // zero ids, so that we'll know later which interfaces were requested
  memset(&this->position_id, 0, sizeof(position_id));
  this->position_subscriptions = 0;
  
  printf( "discovering drivers\n" );
  
  // Do we create a 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;
    }
  }

  //::initialize_robot_params();
  
  _fd = -1;
  _tc_num = new int[3];
  _tc_num[0] = 2;
  _tc_num[1] = 0;
  _tc_num[2] = 185;
  this->_serial_port = cf->ReadString(section, "port", ER_DEFAULT_PORT);
	_need_to_set_speed = true;
	
  //read in axle radius
	_axle_length = cf->ReadFloat( section, "axle", ER_DEFAULT_AXLE_LENGTH );

  //read in left motor and right motor direction
  int dir = cf->ReadInt(section,"motor_dir", 1);
	_motor_0_dir = dir * ER_DEFAULT_MOTOR_0_DIR;
	_motor_1_dir = dir * ER_DEFAULT_MOTOR_1_DIR;

	_debug = 1== cf->ReadInt( section, "debug", 0 );

}
/////////////////////
// bus fcns 
/////////////////////

int
ER::ReadBuf(unsigned char* s, size_t len)
{

	int thisnumread;
	size_t numread = 0;

  while(numread < len)
  {
//  	printf( "len=%d numread=%d\n", len, numread );
    if((thisnumread = read(this->_fd,s+numread,len-numread)) < 0)
    {
      printf("read() failed: %s\n", strerror(errno));
      return(-1);
    }
    if(thisnumread == 0)
      printf("short read\n");
    numread += thisnumread;
  }
  
//  printf("read:\n");
//  for(size_t i=0;i<numread;i++) printf("0x%02x\n", s[i]);
  
  return(0);
}

int
ER::WriteBuf(unsigned char* s, size_t len)
{

  size_t numwritten;
  int thisnumwritten;
  numwritten=0;
  while(numwritten < len)
  {
    if((thisnumwritten = write(this->_fd,s+numwritten,len-numwritten)) < 0)
    {
      if(!this->_fd_blocking && errno == EAGAIN)
      {
        usleep(ER_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 0;
}

int
ER::send_command( unsigned char address, unsigned char c, int ret_num, unsigned char * ret )
{
  
  unsigned char cmd[4];
  
  cmd[0] = address;
  cmd[2] = 0x00;
  cmd[3] = c;
  
  //compute checksum
  int chk = 0x100;
  chk -= cmd[0];
  chk -= cmd[2];
  chk -= cmd[3];
  
  cmd[1] = (unsigned char) chk;
  
  int result = WriteBuf( cmd, 4 );
  
  if( result < 0 )
  {
    printf( "failed to send command\n" );
  }
  
  if( ret > 0 )
  {
    usleep( ER_DELAY_US );
  
    if( ReadBuf( ret, ret_num ) < 0 )
    {
      printf( "failed to read response\n" );
    }
  }


//	PLAYER_WARN1( "cmd: 0x%4x", *((int *) cmd) );

  return result;
}



int
ER::send_command_2_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret )
{
  
  unsigned char cmd[6];
  
  cmd[0] = address;
  cmd[2] = 0x00;
  cmd[3] = c;
  
  int a = htons( arg );
  
  cmd[5] = (a >> 8) & 0xFF;
  cmd[4] = (a >> 0) & 0xFF;
  
  //compute checksum
  int chk = 0x100;
  chk -= cmd[0];
  chk -= cmd[2];
  chk -= cmd[3];
  chk -= cmd[4];
  chk -= cmd[5];
  
  cmd[1] = (unsigned char) chk;
  
  int result = WriteBuf( cmd, 6 );
  
  if( result < 0 )
  {
    printf( "failed to send command\n" );
  }
  
  
  if( ret > 0 )
  {
    usleep( ER_DELAY_US );
    if( ReadBuf( ret, ret_num ) < 0 )
    {
      printf( "failed to read response\n" );
    }
  }
//	PLAYER_WARN1( "cmd: 0x%4x", *((int *) cmd) );
//	PLAYER_WARN1( "cmd: 0x%4x", *((int *) &(cmd[4])) );
  return result;
}

int
ER::send_command_4_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret )
{
  
  unsigned char cmd[8];
  
  cmd[0] = address;
  cmd[2] = 0x00;
  cmd[3] = c;
  
  int a = htonl( arg );
  cmd[7] = (a >> 24) & 0xFF;
  cmd[6] = (a >> 16) & 0xFF;
  cmd[5] = (a >> 8 ) & 0xFF;
  cmd[4] = (a >> 0 ) & 0xFF;
  
  //compute checksum
  int chk = 0x100;
  chk -= cmd[0];
  chk -= cmd[2];
  chk -= cmd[3];
  chk -= cmd[4];
  chk -= cmd[5];
  chk -= cmd[6];
  chk -= cmd[7];
  
  cmd[1] = (unsigned char) chk;
  
  int result = WriteBuf( cmd, 8 );
  
  if( result < 0 )
  {
    printf( "failed to send command\n" );
  }
  
  if( ret > 0 )
  {
    usleep( ER_DELAY_US );
    if( ReadBuf( ret, ret_num ) < 0 )
    {
      printf( "failed to read response\n" );
    }
  }
//	PLAYER_WARN1( "cmd: 0x%4x", *((int *) cmd) );
//	PLAYER_WARN1( "cmd: 0x%4x", *((int *) &(cmd[4])) );
  
  return result;
}

//////////////////////////////
// robot initializations
//////////////////////////////

int
ER::InitRobot()
{

  // initialize the robot
  unsigned char buf[6];
  usleep(ER_DELAY_US);
  if(send_command( MOTOR_0, GETVERSION, 6, buf ) < 0)
  {
    printf("failed to initialize robot\n");
    return -1;
  }

  /* TODO check return value to match 0x00A934100013 */
  if(send_command( MOTOR_1, GETVERSION, 6, buf ) < 0)
  {
    printf("failed to initialize robot\n");
    return -1;
  }
  /* TODO check return value to match 0x00A934100013 */

  _tc_num[2] = 25;
  _stopped = true;
  return(0);
}

int
ER::InitOdom()
{

  unsigned char ret[8];
  
  //try leaving the getVersion out
  send_command( MOTOR_0, RESET, 2, ret );
  send_command_2_arg( MOTOR_0, SETMOTORCMD, 0, 2, ret );
  send_command_2_arg( MOTOR_0, SETLIMITSWITCHMODE, 0, 2, ret );
  send_command_2_arg( MOTOR_0, SETPROFILEMODE, 0x0001, 2, ret );
  send_command_4_arg( MOTOR_0, SETVEL, 0, 2, ret );  
  send_command_4_arg( MOTOR_0, SETACCEL, 0, 2, ret );  
  send_command_4_arg( MOTOR_0, SETDECEL, 0, 2, ret );  
  
  //same for motor 1
  send_command( MOTOR_1, RESET, 2, ret );
  send_command_2_arg( MOTOR_1, SETMOTORCMD, 0, 2, ret );
  send_command_2_arg( MOTOR_1, SETLIMITSWITCHMODE, 0, 2, ret );
  send_command_2_arg( MOTOR_1, SETPROFILEMODE, 0x0001, 2, ret );
  send_command_4_arg( MOTOR_1, SETVEL, 0, 2, ret );  
  send_command_4_arg( MOTOR_1, SETACCEL, 0, 2, ret );  
  send_command_4_arg( MOTOR_1, SETDECEL, 0, 2, ret );  


  //update values
  send_command( MOTOR_0, UPDATE, 2, ret );
  send_command( MOTOR_1, UPDATE, 2, ret );

	_last_ltics = 0;
	_last_rtics = 0;
	
	return 0;
}

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

  this->_px = this->_py = this->_pa = 0.0;
  this->_odom_initialized = false;

  printf("Evolution Robotics evolution_rcm connection initializing (%s)...\n", _serial_port);
  fflush(stdout);

  // 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, B230400);
  cfsetospeed(&term, B230400);
  if(tcsetattr(this->_fd, TCSADRAIN, &term) < 0 )
  {
    printf("tcsetattr() failed: %s\n", strerror(errno));
    close(this->_fd);
    this->_fd = -1;
    return(-1);
  }

  _fd_blocking = false;
  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;

	/*  This might be a good time to reset the odometry values */
	if( InitOdom() < 0 ) {
		printf("InitOdom failed\n" );
		close(this->_fd);
		this->_fd = -1;
		return -1;
	}

	// start the thread to talk with the robot
	this->StartThread();

	return(0);
}


int
ER::Shutdown()
{

  if(this->_fd == -1)
    return(0);

  StopThread();

  // the robot is stopped by the thread cleanup function StopRobot(), which
  // is called as a result of the above StopThread()
  
  if(SetVelocity(0,0) < 0)
    printf("failed to stop robot while shutting down\n");
  

  usleep(ER_DELAY_US);

  if(close(this->_fd))
    printf("close() failed:%s\n",strerror(errno));
  this->_fd = -1;
  if( _debug )
    printf("ER has been shutdown\n\n");
  
  return(0);
}
void
ER::Stop( int StopMode ) {
	
	unsigned char ret[8];
	
	printf( "Stop\n" );
	/* Start with motor 0*/
	_stopped = true;
  if( StopMode == FULL_STOP )
  {
    /* motor 0 */
    send_command_2_arg( MOTOR_0, RESETEVENTSTATUS, 0x0000, 2, ret );