39 #include <libplayercore/playercore.h>
40 #include <replace/replace.h>
43 #define MOTOR_DEF_MAX_SPEED 0.3
44 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
45 #define ACCELERATION_DEFAULT 100
46 #define DECELERATION_DEFAULT 250
53 #define GETCMDPOS 0x1D
54 #define GETCMDVEL 0x1E
55 #define GETEVENTSTATUS 0x31
56 #define RESETEVENTSTATUS 0x34
57 #define GETACTUALPOS 0x37
59 #define SETACTUALPOS 0x4D
60 #define GETSAMPLETIME 0x61
61 #define SETPHASECOUNTS 0x75
62 #define SETMOTORCMD 0x77
63 #define SETLIMITSWITCHMODE 0x80
64 #define WRITEDIGITAL 0x82
65 #define READDIGITAL 0x83
66 #define GETVERSION 0x8F
69 #define SETPROFILEMODE 0xA0
70 #define GETACTUALVEL 0xAD
71 #define SETSTOPMODE 0xD0
72 #define SETMOTORMODE 0xDC
73 #define SETOUTPUTMODE 0xE0
74 #define READANALOG 0xEF
76 #define MOTOR_0 ((unsigned char)0x00)
77 #define MOTOR_1 ((unsigned char)0x01)
80 #define LEFT_MOTOR MOTOR_1
81 #define RIGHT_MOTOR MOTOR_0
84 #define DEFAULT_M3_PORT "/dev/ttyUSB0"
86 #define DELAY_US 10000
89 TrapezoidalProfile = 0,
90 VelocityContouringProfile,
101 #define DEFAULT_MOTOR_0_DIR -1
102 #define DEFAULT_MOTOR_1_DIR 1
103 #define DEFAULT_AXLE_LENGTH (0.301)
105 #define MAX_TICKS 48000
106 #define WHEEL_DIAMETER (0.125)
107 #define WHEEL_RADIUS (WHEEL_DIAMETER/2.0)
108 #define WHEEL_CIRC (M_PI*WHEEL_DIAMETER)
109 #define MOTOR_STEP (1.8/MOTOR_TICKS_PER_STEP)
110 #define GEAR_RATIO (4.8)
111 #define WHEEL_STEP (MOTOR_STEP/GEAR_RATIO)
112 #define M_PER_TICK (WHEEL_RADIUS/WHEEL_STEP)
113 #define MOTOR_TICKS_PER_STEP (64.0)
114 #define MOTOR_TICKS_PER_REV (200.0*MOTOR_TICKS_PER_STEP)
115 #define NUM_IR_SENSORS 8
118 #define MAX_WHEELSPEED 8000
119 #define MPS_PER_TICK 1 // TODO: what is this?
124 #define DEFAULT_PERCENT_TORQUE 75
130 player_position2d_data_t position;
132 player_aio_data_t aio;
133 player_dio_data_t dio;
160 bool RecvBytes(
unsigned char*s,
int len );
161 int ReadBuf(
unsigned char* s,
size_t len);
162 int WriteBuf(
unsigned char* s,
size_t len);
163 int sendCmdCom(
unsigned char address,
unsigned char c,
164 int cmd_num,
unsigned char* arg,
165 int ret_num,
unsigned char * ret );
166 int sendCmd0(
unsigned char address,
unsigned char c,
167 int ret_num,
unsigned char * ret );
168 int sendCmd16(
unsigned char address,
unsigned char c,
169 int16_t arg,
int ret_num,
unsigned char * ret );
170 int sendCmd32(
unsigned char address,
unsigned char c,
171 int32_t arg,
int ret_num,
unsigned char * ret );
173 int32_t BytesToInt32(
unsigned char *ptr );
174 int16_t BytesToInt16(
unsigned char *ptr );
176 int ResetRawPositions();
183 void HandleVelocityCommand(player_position2d_cmd_vel_t* cmd );
184 void HandleDigitalOutCommand( player_dio_data_t* doutCmd );
185 void SetDigitalData( player_dio_data_t * d );
188 void GetAllData(
void );
189 void GetPositionData( player_position2d_data_t* d );
190 void GetIRData( player_ir_data_t * d );
191 void GetAnalogData( player_aio_data_t * d );
192 void GetDigitalData( player_dio_data_t * d );
194 void PublishData(
void);
197 const char* GetPMDErrorString(
int rc );
200 void Stop(
int StopMode );
202 bool EnableMotors(
bool enable );
204 void SetVelocity( uint8_t chan,
float mps );
205 void SetVelocity(
float mpsL,
float mpsR );
206 void SetVelocityInTicks( int32_t left, int32_t right );
207 void GetVelocityInTicks( int32_t* left, int32_t* right );
209 void Move( uint8_t chan,
float meters );
210 void Move(
float metersL,
float metersR );
212 void SetPosition( uint8_t chan,
float meters );
213 void SetPosition(
float metersL,
float metersR );
214 void SetActualPositionInTicks( int32_t left, int32_t right );
215 void SetActualPosition(
float left,
float right );
216 void GetPositionInTicks( int32_t* left, int32_t* right );
218 void SetAccelerationProfile();
220 int GetAnalogSensor(
int s,
short * val );
221 void GetDigitalIn(
unsigned short* digIn );
222 void SetDigitalOut(
unsigned short digOut );
223 void SetOdometry( player_position2d_set_odom_req_t* od );
224 void SetContourMode( ProfileMode_t prof );
225 void SetMicrosteps();
229 int32_t Meters2Ticks(
float meters );
230 float Ticks2Meters( int32_t ticks );
231 int32_t MPS2Vel(
float mps );
232 float Vel2MPS( int32_t vel );
237 struct termios _old_tio;
242 const char* _serial_port;
253 int position_subscriptions;
254 int ir_subscriptions;
255 int aio_subscriptions;
256 int dio_subscriptions;
259 int direct_wheel_vel_control;
261 player_position2d_cmd_vel_t last_position_cmd;
265 int motor_max_turnspeed;
268 short motor_max_trans_accel, motor_max_trans_decel;
269 short motor_max_rot_accel, motor_max_rot_decel;
273 player_position2d_geom_t _robot2d_geom;
274 player_position3d_geom_t _robot3d_geom;
275 player_ir_pose_t _ir_geom;
293 uint16_t _lastDigOut;
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:90
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition: wbr914.cc:538
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
virtual void Publish(player_devaddr_t addr, QueuePointer &queue, uint8_t type, uint8_t subtype, void *src=NULL, size_t deprecated=0, double *timestamp=NULL, bool copy=true)
Publish a message via one of this driver's interfaces.
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
double px
X [m].
Definition: player.h:231
A pose in the plane.
Definition: player.h:218
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: wbr914.cc:674
double ReadTupleLength(int section, const char *name, int index, double value)
Read a length from a tuple (includes units conversion)
Generic message header.
Definition: player.h:162
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
A line segment, used to construct vector-based maps.
Definition: player.h:292
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
virtual void Main(void)=0
Main method for driver thread.
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
const char * ReadFilename(int section, const char *name, const char *value)
Read a filename.
int ReadInt(int section, const char *name, int value)
Read an integer value.
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
void ProcessMessages(void)
Process pending messages.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:285
virtual void Main()
Main method for driver thread.
Definition: wbr914.cc:620
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
static bool MatchDeviceAddress(player_devaddr_t addr1, player_devaddr_t addr2)
Compare two addresses.
Definition: device.h:201
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition: wbr914.cc:509
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
double ReadTupleAngle(int section, const char *name, int index, double value)
Read an angle form a tuple (includes units conversion)
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: wbr914.cc:491
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Class for loading configuration file information.
Definition: configfile.h:197
virtual int Setup()
Initialize the driver.
Definition: driver.h:386
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
A device address.
Definition: player.h:146
An autopointer for the message queue.
Definition: message.h:74
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:82
bool Empty()
Check whether a queue is empty.
Definition: message.h:345
A pose in space.
Definition: player.h:229
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
A point in the plane.
Definition: player.h:185
Messages between wsn and a robot.
Definition: er.h:87
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Reference-counted message objects.
Definition: message.h:133
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
virtual int Shutdown()
Finalize the driver.
Definition: driver.h:393
virtual ~wbr914()
Clean up any resources.
Definition: wbr914.cc:309
Base class for all drivers.
Definition: driver.h:109
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:105
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:186
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: wbr914.cc:320
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76