erratic.h
1// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
2
26#ifndef _ERRATICDEVICE_H
27#define _ERRATICDEVICE_H
28
29#ifndef ERRATIC_VERSION
30#define ERRATIC_VERSION "1.0b"
31#endif
32
33#ifndef ERRATIC_DATE
34#define ERRATIC_DATE "2006-05-07"
35#endif
36
37#include <pthread.h>
38#include <sys/time.h>
39#include <queue>
40
41#include <libplayercore/playercore.h>
42#include <replace/replace.h>
43
44#include "packet.h"
45#include "robot_params.h"
46
47//#include <stdint.h>
48
49#define CPU_VOLTAGE 3.5
50
51// angular constants, angular units are 4096 / rev
52#define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
53#define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
54#define RTOA(x) ((short)((x) * 2048.0) / M_PI)
55
56// Default max speeds
57#define MOTOR_DEF_MAX_SPEED 0.5
58#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
59
60// This merely sets a delay policy in the initial connection
61#define ROBOT_CYCLETIME 20000
62
63/* Erratic constants */
64
65#define VIDERE_NOMINAL_VOLTAGE 12.0
66
67
68// Commands for the robot
69typedef enum command {
70 pulse = 0,
71 open_controller = 1,
72 close_controller = 2,
73 enable_motors = 4,
74 set_max_trans_acc = 5,
75 set_max_position_velocity = 6,
76 reset_origo = 7,
77 trans_vel = 11, // mm/s
78 rot_pos = 12, // deg
79 rot_dpos = 13, // deg
80 configuration = 18,
81 rot_vel = 21, // deg/s
82 set_max_rot_acc = 23,
83 set_sonar = 28,
84 stop = 29,
85 wheel_vel = 32, // mm/s
86 set_analog = 71,
87 save_config = 72,
88 set_pwm_freq = 73,
89 set_pwm_max_on = 74,
90 servo_pos = 75,
91 set_pid_trans_p = 80,
92 set_pid_trans_v = 81,
93 set_pid_trans_i = 82,
94 set_pid_rot_p = 83,
95 set_pid_rot_v = 84,
96 set_pid_rot_i = 85
97
98} command_e;
99
100// Argument types used in robot commands
101typedef enum argtype {
102 argint = 0x3B,
103 argnint = 0x1B,
104 argstr = 0x2B
105} argtype_e;
106
107// Types of replies from the robot
108typedef enum reply {
109 debug = 0x15,
110 config = 0x20,
111 stopped = 0x32,
112 moving = 0x33,
113 motor = 0x80,
114 encoder = 0x90,
115 ain = 0x9a,
116 sonar = 0x9b
117} reply_e;
118
119
120#define DEFAULT_VIDERE_PORT "/dev/erratic"
121
123{
124 player_position2d_data_t position;
125 player_power_data_t power;
126 player_aio_data_t aio;
127 player_ir_data ir;
128 player_sonar_data sonar;
129} __attribute__ ((packed)) player_erratic_data_t;
130
131// this is here because we need the above typedef's before including it.
132#include "motorpacket.h"
133
134extern bool debug_mode;
135
137
138class Erratic : public Driver
139{
140private:
141 int mcount;
142 player_erratic_data_t erratic_data;
143
144 player_devaddr_t position_id;
145 player_devaddr_t power_id;
146 player_devaddr_t aio_id;
147 player_devaddr_t ir_id;
148 player_devaddr_t sonar_id;
149 player_devaddr_t ptz_id, ptz2_id;
150
151 int position_subscriptions;
152 int aio_ir_subscriptions;
153 int sonar_subscriptions;
154 int ptz_subscriptions;
155 int ptz2_subscriptions;
156
157 //ErraticMotorPacket* sippacket;
158 ErraticMotorPacket *motor_packet;
159 pthread_mutex_t motor_packet_mutex;
160
161 int Connect();
162 int Disconnect();
163
164 void ResetRawPositions();
165 void ToggleMotorPower(unsigned char val);
166
167 void ToggleAIn(unsigned char val);
168 void ToggleSonar(unsigned char val);
169
170 int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
171 int HandleCommand(player_msghdr * hdr, void * data);
172 void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
173 void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
174 void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id);
175
176 void PublishAllData();
177 void PublishPosition2D();
178 void PublishPower();
179 void PublishAIn();
180 void PublishIR();
181 void PublishSonar();
182
183 float IRRangeFromVoltage(float voltage);
184 float IRFloorRange(float value);
185
186 void StartThreads();
187 void StopThreads();
188
189 void Send(ErraticPacket *packet);
190 void SendThread();
191 static void *SendThreadDummy(void *driver);
192 void ReceiveThread();
193 static void *ReceiveThreadDummy(void *driver);
194
195 int read_fd, write_fd;
196 const char* psos_serial_port;
197
198 player_position2d_cmd_vel_t last_position_cmd;
199 player_position2d_cmd_car_t last_car_cmd;
200
201 std::queue<ErraticPacket *> send_queue;
202 pthread_mutex_t send_queue_mutex;
203 pthread_cond_t send_queue_cond;
204
205 pthread_t send_thread;
206 pthread_t receive_thread;
207
208 // Parameters
209
210 bool direct_wheel_vel_control;
211
212 bool print_all_packets;
213 bool print_status_summary;
214
215 bool save_settings_in_robot;
216
217 int param_idx; // index in the RobotParams table for this robot
218
219 // Max motor speeds (mm/sec,deg/sec)
220 int motor_max_speed;
221 int motor_max_turnspeed;
222
223 // Customized control settings for the robot
224 int16_t pid_trans_p, pid_trans_v, pid_trans_i;
225 int16_t pid_rot_p, pid_rot_v, pid_rot_i;
226
227 // This is a fairly low-level setting that is exposed
228 uint16_t motor_pwm_frequency, motor_pwm_max_on;
229
230 // Bound the command velocities
231 bool use_vel_band;
232
233 // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
234 short motor_max_trans_accel, motor_max_trans_decel;
235 short motor_max_rot_accel, motor_max_rot_decel;
236
237public:
238
239 Erratic(ConfigFile* cf, int section);
240
241 virtual int Subscribe(player_devaddr_t id);
242 virtual int Unsubscribe(player_devaddr_t id);
243
244 /* the main thread */
245 virtual void Main();
246
247 virtual int Setup();
248 virtual int Shutdown();
249
250 // MessageHandler
251 virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
252};
253
254
255#endif
Class for loading configuration file information.
Definition configfile.h:197
Base class for all drivers.
Definition driver.h:109
Definition erratic/motorpacket.h:38
Definition erratic/packet.h:42
Definition erratic.h:139
virtual int Shutdown()
Finalize the driver.
Definition erratic.cc:707
void ReceiveThread()
Talking to the robot.
Definition erratic.cc:841
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition erratic.cc:802
virtual void Main()
Talking to the Player architecture.
Definition erratic.cc:1122
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition erratic.cc:773
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition erratic.cc:1238
virtual int Setup()
Initialize the driver.
Definition erratic.cc:349
An autopointer for the message queue.
Definition message.h:74
Messages between wsn and a robot.
Definition er.h:87
A device address.
Definition player.h:146
Definition erratic.h:123
Generic message header.
Definition player.h:162