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
113typedef 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
137typedef enum argtype {
138 argint = 0x3B,
139 argnint = 0x1B,
140 argstr = 0x2B
141} argtype_e;
142
143// Types of replies from the robot
144typedef 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
171extern bool debug_mbasedriver;
172extern bool debug_send;
173extern bool debug_receive_aio;
174extern bool debug_receive_sonar;
175extern bool debug_receive_motor;
176extern bool debug_susbcribe;
177extern bool debug_mbase_send_msj;
178
180
181class 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);
212 void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
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
Class for loading configuration file information.
Definition configfile.h:197
Base class for all drivers.
Definition driver.h:109
An autopointer for the message queue.
Definition message.h:74
Copyright (C) 2010 Ana Teresa Hernández Malagón anat.hernandezm@gmail.com Movirobotics athernandez@mo...
Definition mbase/motorpacket.h:46
Definition mbase/packet.h:49
Definition mbasedriver.h:182
mbasedriver(ConfigFile *cf, int section)
Definition mbasedriver.cc:209
void PublishPower()
PublishPower.
Definition mbasedriver.cc:1275
static void * ReceiveThreadDummy(void *driver)
ReceiveThreadDummy.
Definition mbasedriver.cc:1030
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe.
Definition mbasedriver.cc:812
void PublishAllData()
PublishAllData.
Definition mbasedriver.cc:1331
int Disconnect()
Disconnect.
Definition mbasedriver.cc:703
int HandleConfig(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
HandleConfig.
Definition mbasedriver.cc:1361
static void * SendThreadDummy(void *driver)
SedThreadDummy.
Definition mbasedriver.cc:1079
void PublishAIn()
PublishAIn.
Definition mbasedriver.cc:1289
void ToggleSonar(unsigned char val)
ToggleSonar.
Definition mbasedriver.cc:1172
void StopThreads()
StopThreads.
Definition mbasedriver.cc:755
void SendThread()
SendThread.
Definition mbasedriver.cc:1039
void ResetRawPositions()
ResetRawPositions.
Definition mbasedriver.cc:1102
void StartThreads()
StrarThreads.
Definition mbasedriver.cc:746
virtual int Shutdown()
Shutdown.
Definition mbasedriver.cc:692
virtual int Subscribe(player_devaddr_t id)
Subscribe.
Definition mbasedriver.cc:772
int HandleCommand(player_msghdr *hdr, void *data)
HandleCommand.
Definition mbasedriver.cc:1622
virtual void Main()
Main.
Definition mbasedriver.cc:1195
void Send(mbasedriverPacket *packet)
Send.
Definition mbasedriver.cc:1090
void ReceiveThread()
ReceiveThread.
Definition mbasedriver.cc:849
void ToggleAIn(unsigned char val)
ToggleAIn.
Definition mbasedriver.cc:1150
void ToggleMotorPower(unsigned char val)
ToggleMotorPower.
Definition mbasedriver.cc:1126
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
ProcessMessage.
Definition mbasedriver.cc:1345
int getms()
getms
Definition mbasedriver.cc:1520
void PublishIR()
PublishIR.
Definition mbasedriver.cc:1303
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd)
HandlePositionCommand.
Definition mbasedriver.cc:1532
virtual int Setup()
Setup.
Definition mbasedriver.cc:371
int Connect()
Connect.
Definition mbasedriver.cc:381
void PublishPosition2D()
PublishPosition2D.
Definition mbasedriver.cc:1261
void PublishSonar()
PublishSonar.
Definition mbasedriver.cc:1317
Messages between wsn and a robot.
Definition er.h:87
A device address.
Definition player.h:146
Definition mbasedriver.h:160
Generic message header.
Definition player.h:162