wbr914.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2000
4 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
5 *
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 *
21 */
22
23/*
24 * $Id$
25 *
26 * the P2OS device. it's the parent device for all the P2 'sub-devices',
27 * like gripper, position, sonar, etc. there's a thread here that
28 * actually interacts with P2OS via the serial line. the other
29 * "devices" communicate with this thread by putting into and getting
30 * data out of shared buffers.
31 */
32#ifndef _WBR914_H
33#define _WBR914_H
34
35#include <termios.h>
36#include <pthread.h>
37#include <sys/time.h>
38
39#include <libplayercore/playercore.h>
40#include <replace/replace.h>
41
42// Default max speeds
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
47
48/* PMD3410 command codes */
49#define NOOP 0x00
50#define SETPOS 0x10
51#define SETVEL 0x11
52#define UPDATE 0x1A
53#define GETCMDPOS 0x1D
54#define GETCMDVEL 0x1E
55#define GETEVENTSTATUS 0x31
56#define RESETEVENTSTATUS 0x34
57#define GETACTUALPOS 0x37
58#define RESET 0x39
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
67#define SETACCEL 0x90
68#define SETDECEL 0x91
69#define SETPROFILEMODE 0xA0
70#define GETACTUALVEL 0xAD
71#define SETSTOPMODE 0xD0
72#define SETMOTORMODE 0xDC
73#define SETOUTPUTMODE 0xE0
74#define READANALOG 0xEF
75
76#define MOTOR_0 ((unsigned char)0x00)
77#define MOTOR_1 ((unsigned char)0x01)
78
79/* Robot configuration */
80#define LEFT_MOTOR MOTOR_1
81#define RIGHT_MOTOR MOTOR_0
82
83/* Connection stuff */
84#define DEFAULT_M3_PORT "/dev/ttyUSB0"
85
86#define DELAY_US 10000
87
88typedef enum {
89 TrapezoidalProfile = 0,
90 VelocityContouringProfile,
91 SCurveProfile,
92} ProfileMode_t;
93
94typedef enum {
95 NoStopMode = 0,
96 AbruptStopMode,
97 SmoothStopMode
98} StopMode;
99
100/* robot-specific info */
101#define DEFAULT_MOTOR_0_DIR -1
102#define DEFAULT_MOTOR_1_DIR 1
103#define DEFAULT_AXLE_LENGTH (0.301)
104
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
116
117/* for safety */
118#define MAX_WHEELSPEED 8000
119#define MPS_PER_TICK 1 // TODO: what is this?
120
121#define FULL_STOP 0
122#define STOP 1
123
124#define DEFAULT_PERCENT_TORQUE 75
125
126
127
128typedef struct
129{
130 player_position2d_data_t position;
131 player_ir_data_t ir;
132 player_aio_data_t aio;
133 player_dio_data_t dio;
134} __attribute__ ((packed)) player_data_t;
135
136
137class wbr914 : public ThreadedDriver
138{
139 public:
140
141 wbr914(ConfigFile* cf, int section);
142 virtual ~wbr914();
143
144 virtual int Subscribe(player_devaddr_t id);
145 virtual int Unsubscribe(player_devaddr_t id);
146
147 /* the main thread */
148 virtual void Main();
149
150 virtual int MainSetup();
151 virtual void MainQuit();
152
153 // MessageHandler
154 virtual int ProcessMessage(QueuePointer &resp_queue,
155 player_msghdr * hdr,
156 void * data);
157
158 // Private Member Functions
159 private:
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 );
172
173 int32_t BytesToInt32( unsigned char *ptr );
174 int16_t BytesToInt16( unsigned char *ptr );
175
176 int ResetRawPositions();
177 int HandleConfig(QueuePointer &resp_queue,
178 player_msghdr * hdr,
179 void* data);
180
181 // Command handlers
182 int HandleCommand(player_msghdr * hdr, void * data);
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 );
186
187 // Robot data retrievers
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 );
193
194 void PublishData(void);
195
196 /* Robot commands */
197 const char* GetPMDErrorString( int rc );
198 int InitRobot();
199 void UpdateM3();
200 void Stop( int StopMode );
201
202 bool EnableMotors( bool enable );
203
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 );
208
209 void Move( uint8_t chan, float meters );
210 void Move( float metersL, float metersR );
211
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 );
217
218 void SetAccelerationProfile();
219 void StopRobot();
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();
226
227
228 /* Conversions */
229 int32_t Meters2Ticks( float meters );
230 float Ticks2Meters( int32_t ticks );
231 int32_t MPS2Vel( float mps );
232 float Vel2MPS( int32_t vel );
233
234 // Private Data members
235 private:
236 // Comm info for connection to M3 controller
237 struct termios _old_tio;
238 bool _tioChanged;
239
240 int _fd;
241 bool _fd_blocking;
242 const char* _serial_port; // name of serial port device
243 int _baud;
244
245 player_data_t _data;
246
247 player_devaddr_t position_id;
248 player_devaddr_t ir_id;
249 player_devaddr_t aio_id;
250 player_devaddr_t dio_id;
251
252 // bookkeeping to track whether an interface has been subscribed
253 int position_subscriptions;
254 int ir_subscriptions;
255 int aio_subscriptions;
256 int dio_subscriptions;
257
258 int param_idx; // index in the RobotParams table for this robot
259 int direct_wheel_vel_control; // false -> separate trans and rot vel
260
261 player_position2d_cmd_vel_t last_position_cmd;
262
263 // Max motor speeds (mm/sec,deg/sec)
264 int motor_max_speed;
265 int motor_max_turnspeed;
266
267 // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
268 short motor_max_trans_accel, motor_max_trans_decel;
269 short motor_max_rot_accel, motor_max_rot_decel;
270
271 // Geometry
272 // Robot Geometry
273 player_position2d_geom_t _robot2d_geom;
274 player_position3d_geom_t _robot3d_geom;
275 player_ir_pose_t _ir_geom;
276
277 // Odometry stuff
278 int32_t last_lpos;
279 int32_t last_rpos;
280 double _x;
281 double _y;
282 double _yaw;
283
284 // State
285 bool _stopped;
286 bool _motorsEnabled;
287 int _debug;
288 int _usCycleTime;
289 double _velocityK;
290 double _positionK;
291 int _percentTorque;
292
293 uint16_t _lastDigOut;
294};
295
296
297#endif
Class for loading configuration file information.
Definition configfile.h:197
An autopointer for the message queue.
Definition message.h:74
Base class for drivers which oeprate with a thread.
Definition driver.h:553
Definition wbr914.h:138
virtual void Main()
Main method for driver thread.
Definition wbr914.cc:620
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition wbr914.cc:491
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition wbr914.cc:509
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition wbr914.cc:320
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition wbr914.cc:674
virtual ~wbr914()
Clean up any resources.
Definition wbr914.cc:309
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition wbr914.cc:538
Messages between wsn and a robot.
Definition er.h:87
A device address.
Definition player.h:146
Generic message header.
Definition player.h:162