wbr914.h

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  
00004  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
00005  *                      
00006  *
00007  *  This program is free software; you can redistribute it and/or modify
00008  *  it under the terms of the GNU General Public License as published by
00009  *  the Free Software Foundation; either version 2 of the License, or
00010  *  (at your option) any later version.
00011  *
00012  *  This program is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *  GNU General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU General Public License
00018  *  along with this program; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 /*
00024  * $Id: wbr914.h 4135 2007-08-23 19:58:48Z gerkey $
00025  *
00026  *   the P2OS device.  it's the parent device for all the P2 'sub-devices',
00027  *   like gripper, position, sonar, etc.  there's a thread here that
00028  *   actually interacts with P2OS via the serial line.  the other
00029  *   "devices" communicate with this thread by putting into and getting
00030  *   data out of shared buffers.
00031  */
00032 #ifndef _WBR914_H
00033 #define _WBR914_H
00034 
00035 #include <termios.h>
00036 #include <pthread.h>
00037 #include <sys/time.h>
00038 
00039 #include <libplayercore/playercore.h>
00040 #include <replace/replace.h>
00041 
00042 // Default max speeds
00043 #define MOTOR_DEF_MAX_SPEED 0.3
00044 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00045 #define ACCELERATION_DEFAULT 100
00046 #define DECELERATION_DEFAULT 250
00047 
00048 /* PMD3410 command codes */
00049 #define NOOP                0x00
00050 #define SETPOS              0x10
00051 #define SETVEL              0x11
00052 #define UPDATE              0x1A
00053 #define GETCMDPOS           0x1D
00054 #define GETCMDVEL           0x1E
00055 #define GETEVENTSTATUS      0x31
00056 #define RESETEVENTSTATUS    0x34
00057 #define GETACTUALPOS        0x37
00058 #define RESET               0x39
00059 #define SETACTUALPOS        0x4D
00060 #define GETSAMPLETIME       0x61
00061 #define SETPHASECOUNTS      0x75
00062 #define SETMOTORCMD         0x77
00063 #define SETLIMITSWITCHMODE  0x80
00064 #define WRITEDIGITAL        0x82
00065 #define READDIGITAL         0x83
00066 #define GETVERSION          0x8F
00067 #define SETACCEL            0x90
00068 #define SETDECEL            0x91
00069 #define SETPROFILEMODE      0xA0
00070 #define GETACTUALVEL        0xAD
00071 #define SETSTOPMODE         0xD0
00072 #define SETMOTORMODE        0xDC
00073 #define SETOUTPUTMODE       0xE0
00074 #define READANALOG          0xEF
00075 
00076 #define MOTOR_0             ((unsigned char)0x00)
00077 #define MOTOR_1             ((unsigned char)0x01)
00078 
00079 /* Robot configuration */
00080 #define LEFT_MOTOR          MOTOR_1
00081 #define RIGHT_MOTOR         MOTOR_0
00082 
00083 /* Connection stuff */
00084 #define DEFAULT_M3_PORT "/dev/ttyUSB0"
00085 
00086 #define DELAY_US 10000
00087 
00088 typedef enum {
00089   TrapezoidalProfile = 0,
00090   VelocityContouringProfile,
00091   SCurveProfile,
00092 } ProfileMode_t;
00093 
00094 typedef enum {
00095   NoStopMode = 0,
00096   AbruptStopMode,
00097   SmoothStopMode
00098 } StopMode;
00099 
00100 /* robot-specific info */
00101 #define DEFAULT_MOTOR_0_DIR             -1
00102 #define DEFAULT_MOTOR_1_DIR             1
00103 #define DEFAULT_AXLE_LENGTH             (0.301)
00104 
00105 #define MAX_TICKS               48000
00106 #define WHEEL_DIAMETER          (0.125)
00107 #define WHEEL_RADIUS            (WHEEL_DIAMETER/2.0)
00108 #define WHEEL_CIRC              (M_PI*WHEEL_DIAMETER)
00109 #define MOTOR_STEP              (1.8/MOTOR_TICKS_PER_STEP)
00110 #define GEAR_RATIO              (4.8)
00111 #define WHEEL_STEP              (MOTOR_STEP/GEAR_RATIO)
00112 #define M_PER_TICK              (WHEEL_RADIUS/WHEEL_STEP)
00113 #define MOTOR_TICKS_PER_STEP    (64.0)
00114 #define MOTOR_TICKS_PER_REV     (200.0*MOTOR_TICKS_PER_STEP)
00115 #define NUM_IR_SENSORS          8
00116 
00117 /* for safety */
00118 #define MAX_WHEELSPEED          8000
00119 #define MPS_PER_TICK            1              // TODO: what is this?
00120 
00121 #define FULL_STOP       0
00122 #define STOP            1
00123 
00124 #define DEFAULT_PERCENT_TORQUE  75
00125 
00126 
00127 
00128 typedef struct
00129 {
00130   player_position2d_data_t position;
00131   player_ir_data_t         ir;
00132   player_aio_data_t        aio;
00133   player_dio_data_t        dio;
00134 } __attribute__ ((packed)) player_data_t;
00135 
00136 
00137 class wbr914 : public Driver 
00138 {
00139   public:
00140 
00141     wbr914(ConfigFile* cf, int section);
00142     virtual ~wbr914();
00143 
00144     virtual int  Subscribe(player_devaddr_t id);
00145     virtual int  Unsubscribe(player_devaddr_t id);
00146 
00147     /* the main thread */
00148     virtual void Main();
00149 
00150     virtual int  Setup();
00151     virtual int  Shutdown();
00152 
00153     // MessageHandler
00154     virtual int  ProcessMessage(QueuePointer &resp_queue, 
00155                                 player_msghdr * hdr, 
00156                                 void * data);
00157 
00158   // Private Member Functions
00159   private:
00160     bool RecvBytes( unsigned char*s, int len );
00161     int  ReadBuf(unsigned char* s, size_t len);
00162     int  WriteBuf(unsigned char* s, size_t len);
00163     int  sendCmdCom( unsigned char address, unsigned char c, 
00164                      int cmd_num, unsigned char* arg, 
00165                      int ret_num, unsigned char * ret );
00166     int  sendCmd0( unsigned char address, unsigned char c, 
00167                    int ret_num, unsigned char * ret );
00168     int  sendCmd16( unsigned char address, unsigned char c, 
00169                     int16_t arg, int ret_num, unsigned char * ret );
00170     int  sendCmd32( unsigned char address, unsigned char c, 
00171                     int32_t arg, int ret_num, unsigned char * ret );
00172 
00173     int32_t BytesToInt32( unsigned char *ptr );
00174     int16_t BytesToInt16( unsigned char *ptr );
00175 
00176     int  ResetRawPositions();
00177     int  HandleConfig(QueuePointer &resp_queue,
00178                               player_msghdr * hdr,
00179                               void* data);
00180 
00181     // Command handlers
00182     int  HandleCommand(player_msghdr * hdr, void * data);
00183     void HandleVelocityCommand(player_position2d_cmd_vel_t* cmd );
00184     void HandleDigitalOutCommand( player_dio_data_t* doutCmd );
00185     void SetDigitalData( player_dio_data_t * d );
00186 
00187     // Robot data retrievers
00188     void GetAllData( void );
00189     void GetPositionData( player_position2d_data_t* d );
00190     void GetIRData( player_ir_data_t * d );
00191     void GetAnalogData( player_aio_data_t * d );
00192     void GetDigitalData( player_dio_data_t * d );
00193 
00194     void PublishData(void);
00195 
00196     /* Robot commands */
00197     const char* GetPMDErrorString( int rc );
00198     int  InitRobot();
00199     void UpdateM3();
00200     void Stop( int StopMode );
00201 
00202     bool EnableMotors( bool enable );
00203 
00204     void SetVelocity( uint8_t chan, float mps );
00205     void SetVelocity( float mpsL, float mpsR );
00206     void SetVelocityInTicks( int32_t left, int32_t right );
00207     void GetVelocityInTicks( int32_t* left, int32_t* right );
00208 
00209     void Move( uint8_t chan, float meters );
00210     void Move( float metersL, float metersR );
00211 
00212     void SetPosition( uint8_t chan, float meters );
00213     void SetPosition( float metersL, float metersR );
00214     void SetActualPositionInTicks( int32_t left, int32_t right );
00215     void SetActualPosition( float left, float right );
00216     void GetPositionInTicks( int32_t* left, int32_t* right );
00217 
00218     void SetAccelerationProfile();
00219     void StopRobot();
00220     int  GetAnalogSensor(int s, short * val );
00221     void GetDigitalIn( unsigned short* digIn );
00222     void SetDigitalOut( unsigned short digOut );
00223     void SetOdometry( player_position2d_set_odom_req_t* od );
00224     void SetContourMode( ProfileMode_t prof );
00225     void SetMicrosteps();
00226 
00227       
00228     /* Conversions */
00229     int32_t Meters2Ticks( float meters );
00230     float Ticks2Meters( int32_t ticks );
00231     int32_t MPS2Vel( float mps );
00232     float Vel2MPS( int32_t vel );
00233 
00234  // Private Data members
00235  private:
00236     // Comm info for connection to M3 controller
00237     struct termios     _old_tio;
00238     bool               _tioChanged;
00239 
00240     int                _fd;
00241     bool               _fd_blocking;
00242     const char*        _serial_port; // name of serial port device
00243     int                _baud;
00244 
00245     player_data_t    _data;
00246 
00247     player_devaddr_t position_id;
00248     player_devaddr_t ir_id;
00249     player_devaddr_t aio_id;
00250     player_devaddr_t dio_id;
00251 
00252     // bookkeeping to track whether an interface has been subscribed
00253     int position_subscriptions;
00254     int ir_subscriptions;
00255     int aio_subscriptions;
00256     int dio_subscriptions;
00257 
00258     int param_idx;  // index in the RobotParams table for this robot
00259     int direct_wheel_vel_control; // false -> separate trans and rot vel
00260 
00261     player_position2d_cmd_vel_t last_position_cmd;
00262 
00263     // Max motor speeds (mm/sec,deg/sec)
00264     int motor_max_speed;
00265     int motor_max_turnspeed;
00266 
00267     // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
00268     short motor_max_trans_accel, motor_max_trans_decel;
00269     short motor_max_rot_accel, motor_max_rot_decel;
00270 
00271     // Geometry
00272     // Robot Geometry
00273     player_position2d_geom_t  _robot2d_geom;
00274     player_position3d_geom_t  _robot3d_geom;
00275     player_ir_pose_t          _ir_geom;
00276 
00277     // Odometry stuff
00278     int32_t last_lpos;
00279     int32_t last_rpos;
00280     double  _x;
00281     double  _y;
00282     double  _yaw;
00283 
00284     // State
00285     bool    _stopped;
00286     bool    _motorsEnabled;
00287     int     _debug;
00288     int     _usCycleTime;
00289     double  _velocityK;
00290     double  _positionK;
00291     int     _percentTorque;
00292 
00293     uint16_t _lastDigOut;
00294 };
00295 
00296 
00297 #endif
Last updated 12 September 2005 21:38:45