Example Source Code:
Player Project Driver plus Example Player Client Program
For the WheelCommander WC-132


Description

This example includes a driver for Player 2.x and 3.x, as well as a simple wander example.

We have provided a CMake build file, which is how Player 3.x, drivers, and client programs are built. With it you can build the driver and example program under Ubuntu Linux, Mandriva Linux, Mac OS X Leopard or Snow Leopard (requires many MacPorts packages), and most versions of Microsoft Windows. You will also need our binary libaries for your platform; these will be released soon.



 

Download

wc132_driver.cpp - CPP source
wc132.cfg - Player configuration
wander.cpp - CPP source
Player-Stage.zip - Complete Driver including Example Code

 

Source Code


wc132_driver.cpp


/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2003  
 *     Brian Gerkey, 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
 *
 */

/*
 * Desc: Loadable Player driver for the Nubotics WheelCommander WC-132
 * Author: Pete Skeggs
 * Date: 27 Jan 2009 
 * Updated: 6 Dec 2010 for Player 3.x 
 * derived from multidriver.cc example 
 */


// ONLY if you need something that was #define'd as a result of configure 
// (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so:
/*
#if HAVE_CONFIG_H
  #include <config.h>
#endif
*/
#if !defined (WIN32)
   #include <unistd.h>
#endif
#include <string.h>

#include <libplayercore/playercore.h>

#include <math.h>
#include <netinet/in.h>
#include <NdiCmd.h>
#include <Logger.h>

#define TIMEOUT_MS 500 // number of ms to wait for wc132 to respond
#define ANG_CONVERSION_FACTOR ((2 * 3.1415926 / 360.0)/10)
#define VEL_CONVERSION_FACTOR ((25.4/1000.0)/10)
#define ROT_CONVERSION_FACTOR ANG_CONVERSION_FACTOR
// Normalize angle to domain -pi, pi
#ifndef NORMALIZE
   #define NORMALIZE(z) atan2(sin(z), cos(z))
#endif
#define PLAYER3 1 // undefine for player 2.x

const char* copyright_notice =
"\n * Player Project Driver for Nubotics WheelCommander WC-132 [http://www.nubotics.com]\n"
" * Copyright 2006-2011 Noetic Design, Inc.\n"
" * Released under the GNU General Public License v2.\n";


////////////////////////////////////////////////////////////////////////////////
// The class for the driver
#ifdef PLAYER3
   #pragma message "Player 3.x detected; using ThreadedDriver class"
class WC132Driver : public ThreadedDriver
#else
   #pragma message "Player 2.x detected; using Driver class"
class WC132Driver : public Driver
#endif
{
public:

    // Constructor; need that
   WC132Driver(ConfigFile* cf, int section);

    // Must implement the following methods.
#ifdef PLAYER3
   virtual int MainSetup();
   virtual void MainQuit();
#else
   virtual int Setup();
   virtual int Shutdown();
#endif
   virtual int ProcessMessage(QueuePointer & resp_queue, 
                              player_msghdr * hdr, 
                              void * data);

private:
    // Main function for device thread.
   virtual void Main();
   void ProcessPos2dGeomReq(player_msghdr_t* hdr);
   void ProcessAioData(player_msghdr_t* hdr, player_aio_data_t &data);
   void ProcessDioCommand(player_msghdr_t* hdr, player_dio_cmd_t &data);       // outputs
   void ProcessDioData(player_msghdr_t* hdr, player_dio_data_t &data);         // inputs
   void ProcessPos2dVelCmd(player_msghdr_t* hdr, player_position2d_cmd_vel_t &data);
   void ProcessPos2dPosCmd(player_msghdr_t* hdr, player_position2d_cmd_pos_t &data);
   void UpdateData(void);

    // My position interface
   player_devaddr_t m_position_addr;
   player_position2d_data_t m_pos_data;
   int m_timestamp_ms;
    // My dio interface
   player_devaddr_t m_dio_addr;
    // My aio interface
   player_devaddr_t m_aio_addr;
    // My power interface
   player_devaddr_t m_power_addr;
   player_power_data_t m_power_data;

    // WC-132 specific members
   float voltages[8];
   int m_baud_rate;
   NDICMD_DECLSPEC NdiCmd *pNdiCmd;
   WheelCommander *pWC;

    // robot-specific members
   double m_width;
   double m_length;
   double m_height;

    // operational parameters
   BOOL m_quiet;
};


// A factory creation function, declared outside of the class so that it
// can be invoked without any object context (alternatively, you can
// declare it static in the class).  In this function, we create and return
// (as a generic Driver*) a pointer to a new instance of this driver.
Driver* WC132Driver_Init(ConfigFile* cf, int section)
{
    // Create and return a new instance of this driver
   return((Driver*) (new WC132Driver(cf, section)));
}

// A driver registration function, again declared outside of the class so
// that it can be invoked without object context.  In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void WC132Driver_Register(DriverTable* table)
{
   printf("\n ** wc132_driver plugin v1.0 **");
   if (!player_quiet_startup)
      puts(copyright_notice);
   table->AddDriver("wc132", WC132Driver_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.

/* need the extern to avoid C++ name-mangling  */
extern "C"
{
   int player_driver_init(DriverTable* table)
   {
      puts(" wc132 driver plugin init\n");
      WC132Driver_Register(table);
      return(0);
   }
}


////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
WC132Driver::WC132Driver(ConfigFile* cf, int section)
#ifdef PLAYER3
: ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
#else
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
#endif
{
   pNdiCmd = NULL;
   pWC = NULL;
   m_baud_rate = -1;
   m_timestamp_ms = 0;
   memset(&m_pos_data, 0, sizeof(player_position2d_data_t));

    // Create my position interface
   if (cf->ReadDeviceAddr(&m_position_addr, section, 
                          "provides", PLAYER_POSITION2D_CODE, -1, NULL))
   {
      PLAYER_ERROR("Could not read position2d ID ");
      SetError(-1);
      return;
   }
   if (AddInterface(m_position_addr))
   {
      PLAYER_ERROR("Could not add position2d interface ");
      SetError(-1);    
      return;
   }

    // Create dio interface
   if (cf->ReadDeviceAddr(&m_dio_addr, section, 
                          "provides", PLAYER_DIO_CODE, -1, NULL))
   {
      PLAYER_ERROR("Could not read dio ID ");
      SetError(-1);
      return;
   }
   if (AddInterface(m_dio_addr))
   {
      PLAYER_ERROR("Could not add dio interface ");
      SetError(-1);
      return;
   }

    // Create aio interface
   if (cf->ReadDeviceAddr(&m_aio_addr, section, 
                          "provides", PLAYER_AIO_CODE, -1, NULL))
   {
      PLAYER_ERROR("Could not read aio ID ");
      SetError(-1);
      return;
   }
   if (AddInterface(m_aio_addr))
   {
      PLAYER_ERROR("Could not add aio interface ");
      SetError(-1);
      return;
   }

    // Create power interface
   if (cf->ReadDeviceAddr(&m_power_addr, section,
                          "provides", PLAYER_POWER_CODE, -1, NULL))
   {
      PLAYER_ERROR("could not read power ID ");
      SetError(-1);
      return;
   }

   if (AddInterface(m_power_addr))
   {
      PLAYER_ERROR("could not add power interface");
      SetError(-1);
      return;
   }

   m_baud_rate = cf->ReadInt(section, "baud", 38400);
   m_width = cf->ReadFloat(section, "width", 0.2);
   m_length = cf->ReadFloat(section, "length", 0.2);
   m_height = cf->ReadFloat(section, "height", 0.1);
   m_quiet = cf->ReadBool(section, "quiet", TRUE);
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
#ifdef PLAYER3
int WC132Driver::MainSetup()
#else
int WC132Driver::Setup()
#endif
{
   PLAYER_MSG0(0, "wc132driver initialising\n");

    // instantiate the interface library; this will find all active I2C master devices and serial ports
   NdiCmd::Setup(NULL);
   pNdiCmd = NdiCmd::NdiCmdFactory(); 
   if (!pNdiCmd)
   {
      PLAYER_ERROR("Could not open NdiCmd library ");
      return -1;
   }
   if (!pNdiCmd->GetBackplane())
   {
      PLAYER_ERROR("No backplane");
      return -1;
   }

    // override baud rate on all parts to match requested one, if specified; otherwise, use defaults
   if (0)                                                                      // ->PETE m_baud_rate != -1)
   {
      int num_ports = pNdiCmd->GetBackplane()->GetNumSerialPorts();
      for (int i = 0; i < num_ports; i++)
      {
         pNdiCmd->GetBackplane()->SetBaud(i, m_baud_rate);
      }
   }

    // now search for our device
   PLAYER_MSG0(0, "wc132driver searching for WC-132 devices...\n");
   pNdiCmd->DoSearch();

   if (pNdiCmd->GetNumWheelCommanders() == 0)
   {
      printf("wc132driver: could not find a device!\n");
      //delete pNdiCmd;
      pNdiCmd = NULL;
      PLAYER_ERROR("Could not find a WC-132!");
      return -1;
   }

   pWC = pNdiCmd->GetWheelCommander();

   m_timestamp_ms = System::currentTimeMillis();

   PLAYER_MSG0(0, "wc132driver ready\n");
   pNdiCmd->LogQuiet(m_quiet);
    // Start the device thread; spawns a new thread and executes
    // WC132Driver::Main(), which contains the main loop for the driver.
   StartThread();

   return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
#ifdef PLAYER3
void WC132Driver::MainQuit()
#else
int WC132Driver::Shutdown()
#endif
{
   PLAYER_MSG0(0, "Shutting wc132driver down\n");

    // Stop and join the driver thread
   StopThread();

    // delete the interface library
   if (pNdiCmd)
   {
      if (pWC)
      {
         PLAYER_MSG0(0, "Stopping motors.\n");
         pWC->Coast();
      }
      pWC = NULL;
      pNdiCmd->CloseAllPorts();
      pNdiCmd = NULL;
   }

   PLAYER_MSG0(0, "wc132driver has been shutdown\n");
#ifndef PLAYER3
   return(0);
#endif
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void WC132Driver::Main() 
{
    // The main loop; interact with the device here
   for (;;)
   {
        // test if we are supposed to cancel
      pthread_testcancel();

        // process incoming messages; calls ProcessMessage() on each pending message
      ProcessMessages();

        // poll state of WC-132
      UpdateData();

        // give robot a chance to change state
      usleep(10000);
   }
   if (pWC)
   {
      PLAYER_MSG0(0, "Stopping motors.\n");
      pWC->Coast();
   }
   return;
}


int WC132Driver::ProcessMessage(QueuePointer & resp_queue, 
                                player_msghdr * hdr, 
                                void * data)
{
   if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                             PLAYER_POSITION2D_CMD_POS, m_position_addr))
   {
      assert(hdr->size == sizeof(player_position2d_cmd_pos_t));
      ProcessPos2dPosCmd(hdr, *reinterpret_cast<player_position2d_cmd_pos_t *>(data));
      return(0);
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                                  PLAYER_POSITION2D_CMD_VEL, m_position_addr))
   {
      assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
      ProcessPos2dVelCmd(hdr, *reinterpret_cast<player_position2d_cmd_vel_t *>(data));
      return(0);
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                                  PLAYER_DIO_CMD_VALUES, m_dio_addr))
   {
      ProcessDioCommand(hdr, *reinterpret_cast<player_dio_cmd_t *>(data));
      return(0);
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                  PLAYER_DIO_DATA_VALUES, m_dio_addr))
   {
      ProcessDioData(hdr, *reinterpret_cast<player_dio_data_t *>(data));
      return(0);
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                  PLAYER_AIO_DATA_STATE, m_aio_addr))
   {
      ProcessAioData(hdr, *reinterpret_cast<player_aio_data_t *>(data));
      return(0);
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_POSITION2D_REQ_MOTOR_POWER, m_position_addr))
   {
      this->Publish(m_position_addr, resp_queue,
                    PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
      return 0;
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_POSITION2D_REQ_GET_GEOM, m_position_addr))
   {
      ProcessPos2dGeomReq(hdr);
      return(0);
   }



    // Tell the caller that you don't know how to handle this message
   return(-1);
}

void WC132Driver::UpdateData(void)
{
    //
    // Send out new messages with Driver::Publish()
    //
   int vel;
   int rot;
    //long int dist;
    //int ang;
   int new_timestamp_ms;
   float dt;
   float dx;
   float dy;
   float dtheta;
   float v;
   float w;
   float delta_x;
   float delta_y;
   float delta_theta;
   float theta;

   new_timestamp_ms = System::currentTimeMillis();
   dt = (new_timestamp_ms - m_timestamp_ms) / 1000.0;
   m_timestamp_ms = new_timestamp_ms;

    // query robot's current data
   pWC->GetVelocity(vel, TRUE, TIMEOUT_MS);
   pWC->GetRotation(rot, TRUE, TIMEOUT_MS);
    //pWC->GetDistance(dist);
    //pWC->GetAngle(ang);
   theta = m_pos_data.pos.pa;                                                  // our only knowledge of our angle in the global reference frame

    // calculate velocity and rotation rate of robot in meters and meters/sec
   v = vel * VEL_CONVERSION_FACTOR;
   w = rot * ROT_CONVERSION_FACTOR;
    //PLAYER_MSG3(1, "vel rot deltat %f,%f,%f", v, w, dt);

    // calculate velocity and rotation rate in global reference frame
   dx = v * cos(theta);
   dy = v * sin(theta);
   dtheta = w;

    // calculate change in pose in global reference frame
   delta_x = dx * dt;
   delta_y = dy * dt;
   delta_theta = dtheta * dt;
   theta += delta_theta;
   theta = NORMALIZE(theta);
   if (theta < 0)
      theta += 2 * M_PI;

    // finally, calculate actual pose in global reference frame
   m_pos_data.pos.px += delta_x;
   m_pos_data.pos.py += delta_y;
   m_pos_data.pos.pa = theta;

    // also report velocities
   m_pos_data.vel.px = dx;
   m_pos_data.vel.py = dy;
   m_pos_data.vel.pa = dtheta;
   m_pos_data.stall = 0;

    //PLAYER_MSG3(1, "m_pos_data.pos %f,%f:%f", m_pos_data.pos.px, m_pos_data.pos.py, m_pos_data.pos.pa);
    //PLAYER_MSG3(1, "m_pos_data.vel %f,%f:%f", m_pos_data.vel.px, m_pos_data.vel.py, m_pos_data.vel.pa);

   Publish(m_position_addr, 
           PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
           (void*)&m_pos_data, sizeof(m_pos_data), NULL);

   double volts;
   pWC->ReadServoSupply(volts);
    //PLAYER_MSG1(1, "motor volts %g", volts);

   memset(&m_power_data, 0, sizeof(m_power_data));
   m_power_data.valid = PLAYER_POWER_MASK_VOLTS;
   m_power_data.volts = volts;

   Publish(m_power_addr,
           PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE,
           reinterpret_cast<void*>(&m_power_data), sizeof(m_power_data), NULL);
}

void WC132Driver::ProcessPos2dPosCmd(player_msghdr_t* hdr,
                                     player_position2d_cmd_pos_t &data)
{
    // PLS: implement
}

void WC132Driver::ProcessPos2dVelCmd(player_msghdr_t* hdr,
                                     player_position2d_cmd_vel_t &data)
{
   int vel;
   int rot;
   float v;

   PLAYER_MSG3(1, "vel cmd %f,%f:%f", data.vel.px, data.vel.py, data.vel.pa);

   v = sqrt(data.vel.px * data.vel.px + data.vel.py * data.vel.py);
   vel = (int)(v / VEL_CONVERSION_FACTOR);
   rot = (int)(data.vel.pa / ROT_CONVERSION_FACTOR);
   
   PLAYER_MSG2(1, "sending vel=%d, rot=%d", vel, rot);

   pWC->SetVelocity(vel);
   pWC->SetRotation(rot);
   pWC->Go();
}

void WC132Driver::ProcessDioCommand(player_msghdr_t* hdr,
                                    player_dio_cmd_t &data)
{
   unsigned int i;
   for (i = 0; i < data.count; i++)
      pWC->WriteDIO(i, (data.digout & (1 << i)) != 0);

   Publish(m_dio_addr,
           PLAYER_MSGTYPE_DATA, PLAYER_DIO_CMD_VALUES,
           reinterpret_cast<void*>(&data), sizeof(data), NULL);
}

void WC132Driver::ProcessDioData(player_msghdr_t* hdr,
                                 player_dio_data_t &data)
{
   int bits;
   BOOL bit;
   int i;

   for (i = 0, bits = 0; i < 8; i++)
   {
      if (pWC->ReadDIO(i, bit))
         bits |= bit << i;
   }
   data.count = 8;
   data.bits = bits;

   Publish(m_dio_addr,
           PLAYER_MSGTYPE_DATA, PLAYER_DIO_DATA_VALUES,
           reinterpret_cast<void*>(&data), sizeof(data), NULL);
}

void WC132Driver::ProcessAioData(player_msghdr_t* hdr,
                                 player_aio_data_t &data)
{
   int i;
   double v;

   for (i = 0; i < 8; i++)
   {
      pWC->ReadAIO(i, v);
      voltages[i] = (float)v;
   }

   data.voltages_count = 8;
   data.voltages = voltages;

   Publish(m_aio_addr,
           PLAYER_MSGTYPE_DATA, PLAYER_AIO_DATA_STATE,
           reinterpret_cast<void*>(&data), sizeof(data), NULL);
}

void WC132Driver::ProcessPos2dGeomReq(player_msghdr_t* hdr)
{
   player_position2d_geom_t geom;

   geom.pose.px = m_pos_data.pos.px;                                           // [m]
   geom.pose.py = m_pos_data.pos.py;                                           // [m]
   geom.pose.pz = m_pos_data.pos.pa;                                           // [rad]
   geom.size.sl = m_length;                                                    // [m]
   geom.size.sw = m_width;                                                     // [m]
   geom.size.sh = m_height;                                                    // [m]

   Publish(m_position_addr, 
           PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, 
           &geom, sizeof(geom), NULL);
}




wc132.cfg



# Instantiate the driver

driver
(
  name "wc132" 
  plugin "libwc132_driver"
  provides ["position2d:0" "dio:0" "aio:0" "power:0"]
  baud 115200
)



wander.cpp


/************************************************************************/
/* Wander                                                               */
/* WheelCommander Player Example                                        */
/*                                                                      */
/* Based on the wander example from Jennifer Owen's excellent document  */
/* "How to Use Player/Stage," 2nd Edition, page 55.                     */
/*                                                                      */
/* http://www-users.cs.york.ac.uk/~jowen/player/playerstage-manual.html */
/* See also:                                                            */
/* http://playerstage.sourceforge.net/wiki/Tutorials                    */
/*                                                                      */
/* version 1.0, 12/30/2010                                              */
/************************************************************************/

#include <stdio.h>
#ifndef _WIN32
#include <unistd.h>
#include <termios.h>
#else
#include <conio.h>
#endif
#include <time.h>
#include <sys/types.h>
#include <sys/time.h>
#include <libplayerc++/playerc++.h>

using namespace PlayerCc;

void Wander(double *forwardSpeed, double *turnSpeed)
{
   int maxSpeed = 1;
   int maxTurn = 180;
   double fspeed, tspeed;

   fspeed = rand() % 11;
   fspeed = (fspeed / 10) * maxSpeed;

   tspeed = rand() % (2 * maxTurn);
   tspeed = tspeed - maxTurn;

   *forwardSpeed = fspeed;
   *turnSpeed = tspeed;
}


#ifndef _WIN32
// provide a way on Linux and Mac to detect a keypress and abort wandering...
void init(void)
{
   changemode(1);
}

void done(void)
{
   changemode(0);
}

void changemode(int dir)
{
  static struct termios oldt, newt;

  if ( dir == 1 )
  {
    tcgetattr( STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~( ICANON | ECHO );
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);
  }
  else
    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);
}

int _kbhit (void)
{
  struct timeval tv;
  fd_set rdfs;

  tv.tv_sec = 0;
  tv.tv_usec = 0;

  FD_ZERO(&rdfs);
  FD_SET (STDIN_FILENO, &rdfs);

  select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);
  return FD_ISSET(STDIN_FILENO, &rdfs);

}
#else
void init(void)
{
}
void done(void)
{
}
#endif



int main(int argc, char *argv[])
{
   printf("Player Wander Demo v1.1\n\nStarting up...\n");

   init();

   PlayerClient robot("localhost", 6665);
   Position2dProxy robot_drive(&robot, 0);

   double forwardSpeed, turnSpeed;

   srand(time(NULL));

   robot_drive.SetMotorEnable(1);
   //robot_drive.RequestGeom();

   while(!_kbhit())
   {
      robot.Read();
      Wander(&forwardSpeed, &turnSpeed);
      robot_drive.SetSpeed(forwardSpeed, dtor(turnSpeed));
      sleep(2);
   }
   robot_drive.SetMotorEnable(0);
   printf("Player Wander Demo: done!\n");

   done();

   return 0;
}



 

 © 2004-2013 Noetic Design, Inc. All Rights Reserved. Nubotics, Unicoder, WheelCommander, and WheelWatcher are trademarks of Noetic Design, Inc.