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

/*
 * $Id: clodbuster.cc,v 1.10.2.1 2006/06/07 16:12:49 gerkey Exp $
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_clodbuster clodbuster
 * @brief Clodbuster mobile robot

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

The clodbuster driver controls the Clodbuster robot.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_position2d

@par Requires

- none

@par Supported configuration requests

- PLAYER_POSITION_SET_ODOM_REQ
- PLAYER_POSITION_GET_GEOM_REQ
- PLAYER_POSITION_MOTOR_POWER_REQ
- PLAYER_POSITION_VELOCITY_MODE_REQ
- PLAYER_POSITION_RESET_ODOM_REQ
- PLAYER_POSITION_SPEED_PID_REQ

@par Configuration file options

- port (string)
  - Default: ""/dev/ttyUSB0" 
  - Serial port used to communicate with the robot.
  
@par Example 

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

@author Ben Grocholsky
*/
/** @} */

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

#include <fcntl.h> // POSIX file i/o
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h> 
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h> // socket things...
#include <termios.h> // serial port things

#include "clodbuster.h"
#include "packet.h" // What's this for?
#include "playertime.h"
extern PlayerTime* GlobalTime;

// so we can access the deviceTable and extract pointers to the sonar
// and position objects
#include "driver.h"
#include "drivertable.h"
#include "devicetable.h"
#include "error.h"
#include "replace.h"

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

// a driver registration function
void 
ClodBuster_Register(DriverTable* table)
{
  table->AddDriver("clodbuster",  ClodBuster_Init);
}


ClodBuster::ClodBuster( ConfigFile* cf, int section)
        : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION2D_CODE)
{
  clodbuster_fd = -1;

  speedDemand=0, turnRateDemand=0;

  
  strncpy(clodbuster_serial_port,
          cf->ReadString(section, "port", DEFAULT_CLODBUSTER_PORT),
          sizeof(clodbuster_serial_port));

  // set parameters
  CountsPerRev = 408;
  WheelRadius = 0.076;
  WheelBase = .2921;
  WheelSeparation = .275;
  Kenc = 2*M_PI*WheelRadius/CountsPerRev;
  LoopFreq = 5;

  // set PID gains
  //  Kv = new PIDGains(-1.0,-1.5,0.0,LoopFreq);
  //  Kw = new PIDGains(-0.5112,-1.5,0.0,LoopFreq);
  Kv = new PIDGains(-10,-20.0,0.0,LoopFreq);
  Kw = new PIDGains(-5,-20.0,0.0,LoopFreq);
}

ClodBuster::~ClodBuster()
{
  delete Kv;
  delete Kw;
}

int ClodBuster::Setup()
{
  //  int i;
  // this is the order in which we'll try the possible baud rates. we try 9600
  // first because most robots use it, and because otherwise the radio modem
  // connection code might not work (i think that the radio modems operate at
  // 9600).
  //int baud = B38400;
  //  int numbauds = sizeof(bauds);
  // int currbaud = 0;

  struct termios term;
  //unsigned char command;
  // GRASPPacket packet, receivedpacket;
  int flags;
  //bool sent_close = false;
  
  printf("clodbuster connection initializing (%s)...",clodbuster_serial_port);
  fflush(stdout);

  if((clodbuster_fd = open(clodbuster_serial_port, 
			   O_RDWR | O_SYNC, S_IRUSR | S_IWUSR )) < 0 ) // O_NONBLOCK later..
    {
      perror("ClodBuster::Setup():open():");
      return(1);
    }  
 
  if( tcgetattr( clodbuster_fd, &term ) < 0 )
    {
      perror("ClodBuster::Setup():tcgetattr():");
      close(clodbuster_fd);
      clodbuster_fd = -1;
      return(1);
    }
  
  cfmakeraw( &term );
  term.c_cc[VTIME] = 10; /* wait 1 second on port A */
  term.c_cc[VMIN] = 0;  

  cfsetispeed(&term, B38400);
  cfsetospeed(&term, B38400);
  
  if( tcsetattr( clodbuster_fd, TCSAFLUSH, &term ) < 0 )
    {
      perror("ClodBuster::Setup():tcsetattr():");
      close(clodbuster_fd);
      clodbuster_fd = -1;
      return(1);
    }

  if( tcflush( clodbuster_fd, TCIOFLUSH ) < 0 )
    {
      perror("ClodBuster::Setup():tcflush():");
      close(clodbuster_fd);
      clodbuster_fd = -1;
      return(1);
    }

  if((flags = fcntl(clodbuster_fd, F_GETFL)) < 0)
    {
      perror("ClodBuster::Setup():fcntl()");
      close(clodbuster_fd);
      clodbuster_fd = -1;
      return(1);
    }
  /* turn on blocking mode */
  flags &= ~O_NONBLOCK;
  fcntl(clodbuster_fd, F_SETFL, flags);

  
  usleep(CLODBUSTER_CYCLETIME_USEC);
  
  GRASPPacket packet; 
  // disable motor power
  packet.Build(SET_SLEEP_MODE,SLEEP_MODE_OFF);
  packet.Send(clodbuster_fd);
  // reset odometry
  ResetRawPositions();
 
  direct_command_control = true;
  
  /* now spawn reading thread */
  StartThread();
  return(0);
}

int ClodBuster::Shutdown()
{
  GRASPPacket packet; 

  if(clodbuster_fd == -1)
    {
      return(0);
    }

  StopThread();

  packet.Build(SET_SLEEP_MODE,SLEEP_MODE_OFF);
  packet.Send(clodbuster_fd);
  usleep(CLODBUSTER_CYCLETIME_USEC);

  close(clodbuster_fd);
  clodbuster_fd = -1;
  puts("ClodBuster has been shutdown");
 
  return(0);
}

////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int ClodBuster::ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void * data)
{

  assert(hdr);
  assert(data);
	
  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, device_addr))
  {
    assert(hdr->size == sizeof(player_position2d_set_odom_req_t));
    player_position2d_set_odom_req_t & set_odom_req = *((player_position2d_set_odom_req_t*)data);
    
    this->position_data.pos = set_odom_req.pose;

    Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM);
    return 0;
  }

  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, device_addr))
  {
    player_position2d_geom_t geom;
	
    // TODO : get values from somewhere.
    geom.pose.px = -0.1;//htons((short) (-100));
    geom.pose.py = 0;//htons((short) (0));
    geom.pose.pa = 0;//htons((short) (0));
    geom.size.sw = 0.5;//htons((short) (2 * 250));
    geom.size.sl = 0.45;//htons((short) (2 * 225));

    Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom));
    return 0;
  }

  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, device_addr))
  {
    assert(hdr->size == sizeof(player_position2d_power_config_t));
    player_position2d_power_config_t & power_config = *((player_position2d_power_config_t*)data);
    GRASPPacket packet; 
		
    if(power_config.state==1)
      packet.Build(SET_SLEEP_MODE,SLEEP_MODE_OFF);
    else 
       packet.Build(SET_SLEEP_MODE,SLEEP_MODE_ON);

    packet.Send(clodbuster_fd);

    Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
    return 0;
  }

  /* velocity control mode:
   *   0 = direct wheel velocity control (default)
   *   1 = separate translational and rotational control
   */
  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, device_addr))
  {
    assert(hdr->size == sizeof(player_position2d_velocity_mode_config_t));
    player_position2d_velocity_mode_config_t & velmode_config = *((player_position2d_velocity_mode_config_t*)data);