mbasedriver.h
1 // -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
2 
37 #ifndef _mbasedriverDEVICE_H
38 #define _mbasedriverDEVICE_H
39 
40 #ifndef mbasedriver_VERSION
41 #define mbasedriver_VERSION "2.0"
42 #endif
43 
44 #ifndef mbasedriver_DATE
45 #define mbasedriver_DATE "2010-08-31"
46 #endif
47 
48 #ifndef mbase_author
49 #define mbase_author "Hernandez Malagon, Ana Teresa"
50 #endif
51 
52 
53 #include <pthread.h>
54 #include <sys/time.h>
55 #include <queue>
56 
57 #include <libplayercore/playercore.h>
58 #include <replace/replace.h>
59 
60 #include "packet.h"
61 #include "robot_params.h"
62 
63 //#include <stdint.h>
64 
65 #define GradToRad(x) (M_PI * ((double)(x)) / 180.0)
66 #define RadToGrad(x) ((short)((x) * 180.0) / M_PI)
67 
68 // angular constants, angular units are 4096 / rev
69 #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
70 #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
71 #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
72 
73 // Default max speeds
74 #define MOTOR_DEF_MAX_SPEED 0.5
75 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
76 
77 // This merely sets a delay policy in the initial connection
78 #define ROBOT_CYCLETIME 20000
79 
80 /* mbasedriver constants */
81 #define VIDERE_NOMINAL_VOLTAGE 13.0
82 
83 //mm/s/s
84 #define MOTOR_MAX_TRANS_ACEL 1500
85 #define MOTOR_DEF_TRANS_ACEL 500
86 //gados/s/s
87 #define MOTOR_DEF_MAX_ROT_ACEL 135
88 #define PID_P 10
89 #define PID_I 10
90 #define PID_V 6000
91 #define DRIFFACTOR 0
92 #define ROBOT_WHEEL_MR5 190
93 #define ROBOT_WHEEL_MR7 310
94 #define ROBOT_WIDTH_MR5 410
95 #define ROBOT_WIDTH_MR7 495
96 
97 //constantes para transformas voltage infrarrojo
98 #define INFRAmult 5
99 #define INFRAdiv 4096
100 
101 //constantes para calcular distancia sonar
102 //para incrementar la frecuencia a la que trabaja el micro
103 #define PLL 8
104 #define FREC_OSC_MICRO 3.6864
105 #define FACTOR_ESCALA 256
106 
107 #define tipo_MR7 7
108 #define tipo_MR5 5
109 #define DEBUG 0
110 #define IR_AN 0
111 
112 // Commands for the robot
113 typedef enum command
114 {
115  open_controller = 0x01,//1,
116  close_controller = 0x02,//2,
117  enable_motors = 0x04,//4,
118  reset_origo = 0x07,//7,
119  trans_vel = 0x0B,//11, // mm/s
120  rot_vel = 0x15,//21, // deg/s
121  set_sonar = 0x1C,//28,
122  stop = 0x1D,//29,
123  set_analog = 0x47,//71,
124  set_pid_p = 0x50,// 80,
125  set_pid_i = 0x51,// 81,
126  set_pid_v = 0x52,// 82,
127  set_trans_acel = 0x5A,// 90,
128  set_rot_acel = 0x5B,// 91,
129  set_driffactor = 0x5C,// 92,
130  set_robot_width = 0x5D,// 93,
131  set_robot_wheel = 0x5E,// 94
132  set_odometria = 0x5F,//95
133  set_sensores = 0x60
134 } command_e;
135 
136 // Argument types used in robot commands
137 typedef enum argtype {
138  argint = 0x3B,
139  argnint = 0x1B,
140  argstr = 0x2B
141 } argtype_e;
142 
143 // Types of replies from the robot
144 typedef enum reply {
145  debug = 0x15,
146  config = 0x20,
147  stopped = 0x32,
148  moving = 0x33,
149  motor = 0x80,
150  encoder = 0x90,
151  ain = 0x9a,
152  sonar = 0x9b,
153  sensores = 0x90
154 } reply_e;
155 
156 
157 #define DEFAULT_VIDERE_PORT "/dev/ttyS0"
158 
160 {
161  player_position2d_data_t position;
162  player_power_data_t power;
163  player_aio_data_t aio;
164  player_ir_data ir;
165  player_sonar_data sonar;
166 } __attribute__ ((packed)) player_mbasedriver_data_t;
167 
168 // this is here because we need the above typedef's before including it.
169 #include "motorpacket.h"
170 
171 extern bool debug_mbasedriver;
172 extern bool debug_send;
173 extern bool debug_receive_aio;
174 extern bool debug_receive_sonar;
175 extern bool debug_receive_motor;
176 extern bool debug_susbcribe;
177 extern bool debug_mbase_send_msj;
178 
180 
181 class mbasedriver : public Driver
182 {
183  private:
184  int mcount;
185  player_mbasedriver_data_t mbasedriver_data;
186 
187  player_devaddr_t position_id;
188  player_devaddr_t power_id;
189  player_devaddr_t aio_id;
190  player_devaddr_t ir_id;
191  player_devaddr_t sonar_id;
192 
193  int position_subscriptions;
194  int aio_ir_subscriptions;
195  int sonar_subscriptions;
196 
197  //mbasedriverMotorPacket* sippacket;
198  mbasedriverMotorPacket *motor_packet;
199  pthread_mutex_t motor_packet_mutex;
200 
201  int Connect();
202  int Disconnect();
203 
204  void ResetRawPositions();
205  void ToggleMotorPower(unsigned char val);
206 
207  void ToggleAIn(unsigned char val);
208  void ToggleSonar(unsigned char val);
209 
210  int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
211  int HandleCommand(player_msghdr * hdr, void * data);
213 
214  void PublishAllData();
215  void PublishPosition2D();
216  void PublishPower();
217  void PublishAIn();
218  void PublishIR();
219  void PublishSonar();
220 
221  void StartThreads();
222  void StopThreads();
223 
224  void Send(mbasedriverPacket *packet);
225  void SendThread();
226  static void *SendThreadDummy(void *driver);
227  void ReceiveThread();
228  static void *ReceiveThreadDummy(void *driver);
229 
230  int read_fd, write_fd;
231  const char* psos_serial_port;
232 
233  player_position2d_cmd_vel_t last_position_cmd;
234  player_position2d_cmd_car_t last_car_cmd;
235 
236  std::queue<mbasedriverPacket *> send_queue;
237  pthread_mutex_t send_queue_mutex;
238  pthread_cond_t send_queue_cond;
239 
240  pthread_t send_thread;
241  pthread_t receive_thread;
242 
243  // Parameters
244  bool print_all_packets;
245  int param_idx; // index in the RobotParams table for this robot
246 
247  // Max motor speeds (mm/sec,deg/sec)
248  int motor_max_speed;
249  int motor_max_turnspeed;
250 
251  int getms();
252 
253  //mbase
254  int motor_trans_acel;
255  int motor_rot_acel;
256  int trans_ant;
257  int rot_ant;
258  int16_t pid_p, pid_v, pid_i;
259  int16_t driffactor;
260  int16_t dist_ejes;
261  int16_t diametro;
262  bool debug_usuario;
263  bool ir_analog;
264 
265  public:
266  mbasedriver(ConfigFile* cf, int section);
267 
268  virtual int Subscribe(player_devaddr_t id);
269  virtual int Unsubscribe(player_devaddr_t id);
270 
271  /* the main thread */
272  virtual void Main();
273 
274  virtual int Setup();
275  virtual int Shutdown();
276 
277  // MessageHandler
278  virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
279 };
280 
281 
282 #endif
uint8_t state
FALSE for off, TRUE for on.
Definition: player_interfaces.h:667
position 2d velocity command
Definition: player_interfaces.h:617
void PublishIR()
PublishIR.
Definition: mbasedriver.cc:1303
void PublishSonar()
PublishSonar.
Definition: mbasedriver.cc:1317
virtual int Shutdown()
Shutdown.
Definition: mbasedriver.cc:692
void StopThreads()
StopThreads.
Definition: mbasedriver.cc:755
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.
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe.
Definition: mbasedriver.cc:812
#define PLAYER_IR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:2108
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
int Disconnect()
Disconnect.
Definition: mbasedriver.cc:703
Definition: mbasedriver.h:159
void ReceiveThread()
ReceiveThread.
Definition: mbasedriver.cc:849
uint32_t poses_count
The number of valid poses.
Definition: player_interfaces.h:788
Definition: mbasedriver.h:181
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:230
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
Data AND Request/reply: geometry.
Definition: player_interfaces.h:785
uint32_t poses_count
the number of ir samples returned by this robot
Definition: player_interfaces.h:2133
Generic message header.
Definition: player.h:160
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
ProcessMessage.
Definition: mbasedriver.cc:1345
void PublishAllData()
PublishAllData.
Definition: mbasedriver.cc:1331
#define PLAYER_IR_REQ_POSE
Request/reply subtype: get pose.
Definition: player_interfaces.h:2102
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
#define PLAYER_POSITION2D_REQ_MOTOR_POWER
Request/reply: Motor power.
Definition: player_interfaces.h:496
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd)
HandlePositionCommand.
Definition: mbasedriver.cc:1532
position2d power config
Definition: player_interfaces.h:664
double px
X [m].
Definition: player.h:219
Request/reply: get pose.
Definition: player_interfaces.h:2130
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
void StartThreads()
StrarThreads.
Definition: mbasedriver.cc:746
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:240
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
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 ProcessMessages(void)
Process pending messages.
player_pose3d_t * poses
the pose of each IR detector on this robot
Definition: player_interfaces.h:2135
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:284
double py
Y [m].
Definition: player.h:221
void ToggleAIn(unsigned char val)
ToggleAIn.
Definition: mbasedriver.cc:1150
virtual int Subscribe(player_devaddr_t id)
Subscribe.
Definition: mbasedriver.cc:772
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, rad).
Definition: player_interfaces.h:658
int HandleCommand(player_msghdr *hdr, void *data)
HandleCommand.
Definition: mbasedriver.cc:1622
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
static bool MatchDeviceAddress(player_devaddr_t addr1, player_devaddr_t addr2)
Compare two addresses.
Definition: device.h:200
static void * SendThreadDummy(void *driver)
SedThreadDummy.
Definition: mbasedriver.cc:1079
void PublishPower()
PublishPower.
Definition: mbasedriver.cc:1275
int Connect()
Connect.
Definition: mbasedriver.cc:381
void ToggleMotorPower(unsigned char val)
ToggleMotorPower.
Definition: mbasedriver.cc:1126
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
int Build(unsigned char *data, unsigned char datasize)
Build.
Definition: mbase/packet.cc:319
double sl
Length [m].
Definition: player.h:258
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
position2d command setting velocity and steering angle
Definition: player_interfaces.h:637
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
void PublishAIn()
PublishAIn.
Definition: mbasedriver.cc:1289
Copyright (C) 2010 Ana Teresa Hernández Malagón anat.hernandezm@gmail.com Movirobotics athernandez@...
Definition: mbase/motorpacket.h:45
player_pose2d_t pose
(x, y, yaw) [m, m, rad]
Definition: player_interfaces.h:689
#define PLAYER_SONAR_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:755
#define PLAYER_SONAR_DATA_GEOM
Data subtype: geometry.
Definition: player_interfaces.h:764
Class for loading configuration file information.
Definition: configfile.h:195
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2053
double sw
Width [m].
Definition: player.h:256
A device address.
Definition: player.h:144
void PublishPosition2D()
PublishPosition2D.
Definition: mbasedriver.cc:1261
#define PLAYER_POSITION2D_REQ_RESET_ODOM
Request/reply: Reset odometry.
Definition: player_interfaces.h:541
An autopointer for the message queue.
Definition: message.h:72
double py
Y [m].
Definition: player.h:232
int HandleConfig(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
HandleConfig.
Definition: mbasedriver.cc:1361
mbasedriver(ConfigFile *cf, int section)
Definition: mbasedriver.cc:209
void SetError(int code)
Set/reset error code.
Definition: driver.h:144
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
bool Empty()
Check whether a queue is empty.
Definition: message.h:344
A pose in space.
Definition: player.h:227
int getms()
getms
Definition: mbasedriver.cc:1520
bool Parse(unsigned char *buffer, int length)
Parse -Parses and absorbs a standard packet from the robot.
Definition: mbase/motorpacket.cc:58
Messages between wsn and a robot.
Definition: er.h:86
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
player_pose3d_t * poses
Pose of each sonar, in robot cs.
Definition: player_interfaces.h:790
#define PLAYER_AIO_DATA_STATE
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2046
void SendThread()
SendThread.
Definition: mbasedriver.cc:1039
Data: ranges (PLAYER_IR_DATA_RANGES)
Definition: player_interfaces.h:2115
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:173
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
virtual void Main()
Main.
Definition: mbasedriver.cc:1195
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
static void * ReceiveThreadDummy(void *driver)
ReceiveThreadDummy.
Definition: mbasedriver.cc:1030
double pa
yaw [rad]
Definition: player.h:223
virtual int Setup()
Setup.
Definition: mbasedriver.cc:371
void Send(mbasedriverPacket *packet)
Send.
Definition: mbasedriver.cc:1090
Base class for all drivers.
Definition: driver.h:107
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
int Receive(int fd, uint16_t wait=30)
Receive.
Definition: mbase/packet.cc:130
Definition: mbase/packet.h:48
set odometry
Definition: player_interfaces.h:686
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
void ResetRawPositions()
ResetRawPositions.
Definition: mbasedriver.cc:1102
void Fill(player_mbasedriver_data_t *data)
Fill.
Definition: mbase/motorpacket.cc:111
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
void ToggleSonar(unsigned char val)
ToggleSonar.
Definition: mbasedriver.cc:1172