reb.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
 *
 */

/* Copyright (C) 2002
 *   John Sweeney, UMASS, Amherst, Laboratory for Perceptual Robotics
 *
 * $Id: reb.cc,v 1.13 2006/02/23 18:54:55 gerkey Exp $
 *
 *   The REB device.  This controls the K-Team Kameleon 376SBC with
 * the Robotics Extension Board (REB).  (Technically the REB doesn't control
 * anything, it just provides the analog I/Os, H-bridges, etc but we
 * thought REB was a good acronym...)  The REB/Kameleon board has the motor
 * drivers and sensor I/O, and we communicate with it via a serial port.
 * So the overall architecture is similar to the p2osdevice, where this class
 * handles the data gathering tasks for the Position, IR and power devices.
 *
 * Note that we have actually made our own version of the SerCom program that
 * runs on the Kameleon.  Our version runs faster than K-Teams, so we
 * can reliably get new data at around 10 Hz.  (K-Team SerCom barfed for us
 * faster than about 2 Hz!)  Our SerCom, called LPRSerCom, also handles turning
 * the IRs on and off, so we don't have to worry about that in the player server.
 * If you would like a copy of LPRSerCom, then send me email: sweeney (at) cs.umass.edu
 *
 * Our robots use a StrongARM SA110 for the compute power, so we have
 * to minimize the use of floating point, since the ARM can only emulate
 * it.
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_reb reb
 * @brief K-Team Kameleon Robotics Extension Board

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

The reb driver is used to control robots using the K-Team Kameleon
376SBC with Robotics Extension Board (REB).  The Kameleon, (or Kam),
has a Motorola MC68376 microcontroller that can perform velocity and
position control and odometry for up to four motors, using the REB.
It can also access a number of A/D inputs, which we have connected to
Sharp GP2D12 IR proximity detectors.

In its default setting, a host computer can communicate with the Kam
using the K-Team SerCom program, which uses a simple protocol to send
commands and read data back.  At UMass, we found that the default
SerCom did not offer enough performance, so we developed our own,
LPRSerCom, which uses the same protocol, but with some enhancements,
such as letting the Kam do the odometry updates and IR synchronization.
The bottom line is that you need to modifiy these drivers to work with the
K-Team SerCom, which is not very difficult (mainly removing the LPRSerCom
specific code).  We can also send you a copy of LPRSerCom if you'd like.
Email John Sweeney (sweeney (at) cs.umass.edu) for information.

The reb driver sets some default PID parameters and resets the
odometry to (0,0,0) when the first client subscribes to the @ref
interface_position2d interface.  Likewise, the IR sensors are only
turned on when an @ref interface_ir client has subscribed.

Position mode is very finicky.  This seems to be a problem with the
REB itself, which may lose bytes on the serial port while performing
position mode actions.  This causes the driver to time out, and quite
possibly lose a connection to the REB.

The LPRSerCom protocol running on the REB will sometimes lose a byte
over the port, which can cause the driver to time out on a read call to
the port.  The driver will attempt to retry the call, but there is no
guarantee that the REB will be able to handle it.  The best solution is
to reset the REB.  Hopefully this should be a relatively rare occurrence.

As mentioned above, for this driver to function properly, the REB needs
to be running the LPRSerCom program.  

@par Compile-time dependencies

- none

@par Provides

The reb driver provides the following device interfaces:

- @ref interface_position2d : This interface returns odometry data,
  and accepts velocity commands.

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

- @ref interface_power : This interface returns power data.

@par Supported configuration requests

- The @ref interface_position2d interface supports:
  - PLAYER_POSITION_GET_GEOM_REQ
  - PLAYER_POSITION_MOTOR_POWER_REQ
  - PLAYER_POSITION_VELOCITY_MODE_REQ
  - PLAYER_POSITION_RESET_ODOM_REQ
  - PLAYER_POSITION_POSITION_MODE_REQ
  - PLAYER_POSITION_SET_ODOM_REQ
  - PLAYER_POSITION_SPEED_PID_REQ
  - PLAYER_POSITION_POSITION_PID_REQ
  - PLAYER_POSITION_SPEED_PROF_REQ
- The @ref interface_ir interface supports:
  - PLAYER_IR_POWER_REQ
  - PLAYER_IR_POSE_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttySA1"
  - Serial port used to communicate with the robot.
- subclass (string)
  - Default: "slow"
  - The type of robot; should be "slow" or "fast"

  
@par Example 

@verbatim
driver
(
  name "reb"
  provides ["position2d:0" "ir:0" "power:0"]
)
@endverbatim

@author John Sweeney
*/
/** @} */

#include <fcntl.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h>
#include <ctype.h>

#include <reb.h>

#include <error.h>
#include <playertime.h>
extern PlayerTime* GlobalTime;

// so we can access the deviceTable and extract pointers to the sonar
// and position objects
#include <devicetable.h>
extern int global_playerport; // used to get at devices

// we need to debug different things at different times
//#define DEBUG_POS
//#define DEBUG_SERIAL
#define DEBUG_CONFIG

// useful macros
#define DEG2RAD(x) (((double)(x))*0.01745329251994)
#define RAD2DEG(x) (((double)(x))*57.29577951308232)

//#define DEG2RAD_FIX(x) ((x) * 17453)
//#define RAD2DEG_FIX(x) ((x) * 57295780)
#define DEG2RAD_FIX(x) ((x) * 174)
#define RAD2DEG_FIX(x) ((x) * 572958)

/* initialize the driver.
 *
 * returns: pointer to new REBIR object
 */
Driver*
REB_Init(ConfigFile *cf, int section)
{
  return (Driver *) new REB( cf, section);
}

/* register the Khepera IR driver in the drivertable
 *
 * returns: 
 */
void
REB_Register(DriverTable *table) 
{
  table->AddDriver("reb", REB_Init);
}

REB::REB(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(player_device_id_t));
  memset(&this->ir_id, 0, sizeof(player_device_id_t));
  memset(&this->power_id, 0, sizeof(player_device_id_t));

  last_trans_command=last_rot_command=0;
  leftvel=rightvel=0;
  leftpos=rightpos=0;


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

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

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

  // Do we create a power interface?
  if(cf->ReadDeviceId(&(this->power_id), section, "provides", 
                      PLAYER_POWER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->power_id, PLAYER_READ_MODE) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  // build the table of robot parameters.
  initialize_reb_params();

  // Read config file options
  
  // also, install default parameter values.
  strncpy(reb_serial_port,REB_DEFAULT_SERIAL_PORT,sizeof(reb_serial_port));
  reb_fd = -1;
  param_index = 0;

  //set up the poll parameters... used for the comms
  // over the serial port to the Kam
  write_pfd.events = POLLOUT;
  read_pfd.events = POLLIN;

  // now we have to look up our parameters.  this should be given as an argument
  strncpy(reb_serial_port, cf->ReadString(section, "port", reb_serial_port),
	  sizeof(reb_serial_port));

  char subclass[32] = "slow";
  strncpy(subclass, cf->ReadString(section, "subclass", subclass),
          strlen(subclass));
  if (!strcmp(subclass, "fast")) {
    param_index = 1;
  } else {
    param_index = 0;
  }

  // zero position counters
  last_lpos = 0;
  last_rpos = 0;
  last_x_f=0;
  last_y_f=0;
  last_theta = 0.0;
}

/* called the first time a client connects
 *
 * returns: 0 on success
 */
int 
REB::Setup()
{
  struct termios oldtio;
  struct termios params;

  // open and initialize the serial port from the ARM -> REB  
  printf("REB: connection initializing (%s)...\n", this->reb_serial_port);
  fflush(stdout);

  if ((this->reb_fd = open(this->reb_serial_port, O_RDWR | O_NOCTTY)) == -1) {
    perror("REB::Setup():open()");
    return(1);
  }	

  // set the poll params
  write_pfd.fd = reb_fd;
  read_pfd.fd = reb_fd;
  
  memset(&params, 0, sizeof(params));  
  tcgetattr(this->reb_fd, &oldtio); /* save current serial port settings */
  params.c_cflag = REB_BAUDRATE | CS8 | CLOCAL | CREAD | CSTOPB;
  params.c_iflag = 0; 
  params.c_oflag = 0;
  params.c_lflag = ICANON; 
   
  params.c_cc[VMIN] = 0;
  params.c_cc[VTIME] = 0;
   
  tcflush(this->reb_fd, TCIFLUSH);
  tcsetattr(this->reb_fd, TCSANOW, &params);

  //  Restart();

  // so no IRs firing
  SetIRState(REB_IR_STOP);

  refresh_last_position = false;
  motors_enabled = false;
  velocity_mode = true;
  direct_velocity_control = false;

  desired_heading = 0;

  /*player_position_cmd_t cmd;
  memset(&cmd,0,sizeof(player_position_cmd_t));
  PutData(this->position_id,(void*)&cmd,
          sizeof(player_position_cmd_t),NULL);*/

  /* now spawn reading thread */
  StartThread();
  return(0);
}


int 
REB::Shutdown()
{
  printf("REB: SHUTDOWN\n");

  StopThread();

  SetSpeed(REB_MOTOR_LEFT, 0);
  SetSpeed(REB_MOTOR_RIGHT, 0);

  SetIRState(REB_IR_STOP);

  close(reb_fd);
  reb_fd = -1;
  return(0);
}

int 
REB::Subscribe(player_device_id_t id)
{
  int setupResult;

  // do the subscription
  if((setupResult = Driver::Subscribe(id)) == 0)
  {
    // also increment the appropriate subscription counter
    switch(id.code)
    {
      case PLAYER_POSITION_CODE:
        this->position_subscriptions++;
        break;
      case PLAYER_IR_CODE:
        this->ir_subscriptions++;
        break;
    }
  }

  return(setupResult);
}

int 
REB::Unsubscribe(player_device_id_t id)
{
  int shutdownResult;

  // do the unsubscription
  if((shutdownResult = Driver::Unsubscribe(id)) == 0)
  {
    // also decrement the appropriate subscription counter
    switch(id.code)
    {
      case PLAYER_POSITION_CODE:
        assert(--this->position_subscriptions >= 0);
        break;
      case PLAYER_IR_CODE:
        assert(--this->ir_subscriptions >= 0);
        break;
    }
  }

  return(shutdownResult);
}


void 
REB::Main()
{
  int last_ir_subscrcount=0;
  int last_position_subscrcount=0;


  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

  while (1) {
    // we want to turn on the IR if someone just subscribed, and turn
    // them off if the last subscriber just unsubscribed.
    if(!last_ir_subscrcount && this->ir_subscriptions)
    {
      // then someone just subbed to IR
      SetIRState(REB_IR_START);

      // zero out ranges in IR data so proxy knows
      // to do regression
      player_ir_data_t ir_data;
      memset(&ir_data,0,sizeof(player_ir_data_t));
      PutMsg(this->ir_id,NULL, PLAYER_MSGTYPE_DATA, 0,(unsigned char*)&ir_data,
              sizeof(player_ir_data_t),NULL);
    } 
    else if(last_ir_subscrcount && !this->ir_subscriptions)
    {
      // then last person stopped sub from IR..
      SetIRState(REB_IR_STOP);
    }
    last_ir_subscrcount = this->ir_subscriptions;
    
    // we want to reset the odometry and enable the motors if the first 
    // client just subscribed to the position device, and we want to stop 
    // and disable the motors if the last client unsubscribed.
    if(!last_position_subscrcount && this->position_subscriptions)
    {
      printf("REB: first pos sub. turn off and reset\n");
      // then first sub for pos, so turn off motors and reset odom
      SetSpeed(REB_MOTOR_LEFT, 0);
      SetSpeed(REB_MOTOR_RIGHT, 0);

      SetOdometry(0,0,0);

      // set up speed and pos PID
      ConfigSpeedPID(0, 1000, 0, 10);
      ConfigSpeedPID(2, 1000, 0, 10);
      ConfigPosPID(0, 100, 0, 10);
      ConfigPosPID(2, 100, 0, 10);

      // have to convert spd from mm/s to pulse/10ms
      int spd = (int) rint(100.0 * 
                           PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);

      // have to convert acc from mm/s^2 to pulses/256/(10ms^2)
      int acc = (int) rint(100.0 * 
                           PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);

      if (acc > REB_MAX_ACC) {
        acc = REB_MAX_ACC;
      } else if (acc == 0) {
        acc = REB_MIN_ACC;
      }
      ConfigSpeedProfile(0, spd, acc);
      ConfigSpeedProfile(2, spd, acc);

    } 
    else if(last_position_subscrcount && !this->position_subscriptions)
    {
      // last sub just unsubbed
      printf("REB: last pos sub gone\n");
      SetSpeed(REB_MOTOR_LEFT, 0);
      SetSpeed(REB_MOTOR_RIGHT, 0);

      // overwrite existing motor commands to be zero
      player_position_cmd_t position_cmd;
      memset(&position_cmd,0,sizeof(position_cmd));
      /*PutCommand(this->position_id,
                 (unsigned char*)(&position_cmd), sizeof(position_cmd),NULL);*/
      ProcessCommand(&position_cmd);
    }
    last_position_subscrcount = this->position_subscriptions;

    // get configuration commands (ioctls)
    //ReadConfig();
    ProcessMessages();
 
    pthread_testcancel();

    // now lets get new data...
    UpdateData();

    pthread_testcancel();
    
  }
  pthread_exit(NULL);
}

int REB::ProcessCommand(player_position_cmd_t * poscmd)
{
	player_position_cmd_t & cmd = * poscmd;
    if(this->position_subscriptions) 
    {
      bool newtrans = false, newrot = false, newheading=false;
      bool newposcommand=false;
      short trans_command, rot_command, heading_command;

      if ((trans_command = (short)ntohl(cmd.xspeed)) != 
          last_trans_command) {
        newtrans = true;
        last_trans_command = trans_command;
      }

      if ((rot_command = (short) ntohl(cmd.yawspeed)) != 
          last_rot_command) {
        newrot = true;
        last_rot_command = rot_command;
      }

      if ((heading_command = (short) ntohl(cmd.yaw)) != 
          this->desired_heading) {