Fawkes API Fawkes Development Version
ffptu.cpp
1
2/***************************************************************************
3 * ffptu.cpp - Control PTU via keyboard
4 *
5 * Created: Thu Oct 06 16:28:16 2011
6 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "../robotis/rx28.h"
24
25#include <blackboard/remote.h>
26#include <core/exceptions/system.h>
27#include <interface/interface_info.h>
28#include <interfaces/PanTiltInterface.h>
29#include <logging/console.h>
30#include <utils/system/argparser.h>
31#include <utils/system/getkey.h>
32#include <utils/time/time.h>
33
34#include <cstdio>
35#include <cstdlib>
36#include <cstring>
37#include <unistd.h>
38
39using namespace fawkes;
40
41void
42print_usage(const char *program_name)
43{
44 printf("Usage: %s [-h] [-r host[:port]] <command>\n"
45 " Options:\n"
46 " -h This help message\n"
47 " -r host[:port] Remote host (and optionally port) to connect to\n"
48 " -p <ptu> PTU, interface must have id 'PanTilt <ptu>'\n"
49 " -l List available PTUs (only on remote blackboard)\n"
50 " -i Invert tilt control buttons\n\n"
51 " Commands:\n"
52 "move [pan P] [tilt T]\n"
53 " Move PTU, if either pan or tilt are supplied, send the\n"
54 " command and wait for angle to be reached. Otherwise enter\n"
55 " interactive mode\n"
56 "rx28-set-id <old-id> <new-id>\n"
57 " Send message to set a new servo ID.\n"
58 " WARNING: use this with only a single servo connected!\n"
59 "rx28-discover\n"
60 " Discover and print servos on the BUS.\n",
61 program_name);
62}
63
64/** Remote control PTUs via keyboard.
65 * @author Tim Niemueller
66 */
68{
69public:
70 /** Constructor.
71 * @param argc number of arguments in argv
72 * @param argv array of parameters passed into the program
73 */
74 PTUJoystickControl(int argc, char **argv) : argp_(argc, argv, "hr:p:li")
75 {
76 rx28_ = NULL;
77 bb_ = NULL;
78 ptu_if_ = NULL;
79 resolution_ = 0.1f;
80
81 if (argp_.has_arg("h")) {
82 print_usage(argv[0]);
83 exit(0);
84 }
85 }
86
87 /** Destructor. */
89 {
90 if (bb_) {
91 bb_->close(ptu_if_);
92 delete bb_;
93 }
94 delete rx28_;
95 }
96
97 /** Initialize BB connection. */
98 void
100 {
101 char * host = (char *)"localhost";
102 unsigned short int port = 1910;
103 bool free_host = argp_.parse_hostport("r", &host, &port);
104
105 bb_ = new RemoteBlackBoard(host, port);
106 if (free_host)
107 free(host);
108
109 if (argp_.has_arg("l")) {
110 InterfaceInfoList *l = bb_->list("PanTiltInterface", "PanTilt *");
111 if (l->empty()) {
112 printf("No interfaces found");
113 }
114 for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
115 std::string id = i->id();
116 id = id.substr(std::string("PanTilt ").length());
117
118 printf("PTU: %s Writer: %s\n", id.c_str(), i->has_writer() ? "Yes" : "No");
119 }
120 delete l;
121 return;
122 }
123
124 if (argp_.has_arg("p")) {
125 std::string iface_id = std::string("PanTilt ") + argp_.arg("p");
126 ptu_if_ = bb_->open_for_reading<PanTiltInterface>(iface_id.c_str());
127 } else {
128 InterfaceInfoList *l = bb_->list("PanTiltInterface", "PanTilt *");
129 if (l->empty()) {
130 throw Exception("No PanTilt interface opened on remote!");
131 }
132 for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
133 if (i->has_writer()) {
134 ptu_if_ = bb_->open_for_reading<PanTiltInterface>(i->id());
135 } else {
136 printf("Interface %s has no writer, ignoring\n", i->id());
137 }
138 }
139 delete l;
140 }
141
142 if (!ptu_if_) {
143 throw Exception("No suitable PanTiltInterface found");
144 }
145 }
146
147 /** Initialize Robotis RX28 raw servo access. */
148 void
150 {
151 rx28_ = new RobotisRX28("/dev/ttyUSB0");
152 RobotisRX28::DeviceList devl = rx28_->discover();
153
154 if (devl.empty()) {
155 throw Exception("No devices found\n");
156 }
157 }
158
159 /** Run control loop. */
160 void
162 {
163 if (argp_.has_arg("l")) {
164 init_bb();
165 } else if (argp_.num_items() == 0) {
166 interactive_move();
167 } else if (strcmp(argp_.items()[0], "move") == 0) {
168 if (argp_.num_items() == 1) {
169 interactive_move();
170 } else {
171 exec_move();
172 }
173 } else if (strcmp(argp_.items()[0], "rx28-set-id") == 0) {
174 rx28_set_id();
175
176 } else if (strcmp(argp_.items()[0], "rx28-discover") == 0) {
177 rx28_discover();
178
179 } else {
180 printf("Unknown command '%s'\n", argp_.items()[0]);
181 }
182 }
183
184private:
185 void
186 interactive_move()
187 {
188 init_bb();
189
190 Time last, now;
191
192 char tilt_up = 65;
193 char tilt_down = 66;
194 if (argp_.has_arg("i"))
195 std::swap(tilt_up, tilt_down);
196
197 float pan, tilt, new_pan, new_tilt;
198 float speed = 0.0, new_speed = 0.5;
199
200 ptu_if_->read();
201 pan = new_pan = ptu_if_->pan();
202 tilt = new_tilt = ptu_if_->tilt();
203
204 last.stamp();
205 char key = 0;
206 int wait_time = 5;
207 while (key != 'q') {
208 key = getkey(wait_time);
209 //printf("Key: %u = %u\n", key, key);
210 if (key != 0) {
211 now.stamp();
212 if ((now - &last) < 0.5) {
213 wait_time = 1;
214 }
215 last.stamp();
216 }
217 if (key == 0) {
218 wait_time = 5;
219
220 } else if (key == 27) {
221 key = getkey();
222 if (key == 0) {
223 // Escape key
224 new_pan = pan;
225 new_tilt = tilt;
226 } else {
227 if (key != 91)
228 continue;
229
230 key = getkey();
231 if (key == 0)
232 continue;
233
234 if (key == tilt_up) {
235 new_tilt = std::min(tilt + resolution_, ptu_if_->max_tilt());
236 } else if (key == tilt_down) {
237 new_tilt = std::max(tilt - resolution_, ptu_if_->min_tilt());
238 } else if (key == 67) {
239 new_pan = std::max(pan - resolution_, ptu_if_->min_pan());
240 } else if (key == 68) {
241 new_pan = std::min(pan + resolution_, ptu_if_->max_pan());
242 } else
243 continue;
244 }
245 } else if (key == '0') {
246 new_pan = new_tilt = 0.f;
247 } else if (key == '9') {
248 new_pan = 0;
249 new_tilt = M_PI / 2.;
250 } else if (key == 'r') {
251 resolution_ = 0.1f;
252 } else if (key == 'R') {
253 resolution_ = 0.01f;
254 } else if (key == '+') {
255 new_speed = std::min(speed + 0.1, 1.0);
256 } else if (key == '-') {
257 new_speed = std::max(speed - 0.1, 0.0);
258 }
259
260 if (speed != new_speed) {
261 speed = new_speed;
262 float pan_vel = speed * ptu_if_->max_pan_velocity();
263 float tilt_vel = speed * ptu_if_->max_tilt_velocity();
264
265 printf("Setting velocity %f/%f (max %f/%f)\n",
266 pan_vel,
267 tilt_vel,
268 ptu_if_->max_pan_velocity(),
269 ptu_if_->max_tilt_velocity());
270
272 new PanTiltInterface::SetVelocityMessage(pan_vel, tilt_vel);
273 ptu_if_->msgq_enqueue(svm);
274 }
275
276 if ((pan != new_pan) || (tilt != new_tilt)) {
277 pan = new_pan;
278 tilt = new_tilt;
279
280 printf("Goto %f/%f\n", pan, tilt);
281
283 ptu_if_->msgq_enqueue(gm);
284 }
285 }
286 }
287
288 void
289 exec_move()
290 {
291 init_bb();
292
293 float pan, tilt, new_pan, new_tilt;
294
295 ptu_if_->read();
296 pan = new_pan = ptu_if_->pan();
297 tilt = new_tilt = ptu_if_->tilt();
298
299 const std::vector<const char *> &items = argp_.items();
300 for (unsigned int i = 1; i < items.size(); ++i) {
301 if (strcmp(items[i], "pan") == 0) {
302 if (items.size() > i + 1) {
303 new_pan = argp_.parse_item_float(++i);
304 } else {
305 printf("No pan value supplied, aborting.\n");
306 return;
307 }
308 } else if (strcmp(items[i], "tilt") == 0) {
309 if (items.size() > i + 1) {
310 new_tilt = argp_.parse_item_float(++i);
311 } else {
312 printf("No tilt value supplied, aborting.\n");
313 return;
314 }
315 } else {
316 printf("Unknown parameter '%s', aborting.\n", items[i]);
317 return;
318 }
319 }
320
321 if ((pan != new_pan) || (tilt != new_tilt)) {
322 printf("Goto pan %f and tilt %f\n", new_pan, new_tilt);
323
326 ptu_if_->max_tilt_velocity() / 2.);
327 ptu_if_->msgq_enqueue(svm);
328
330 ptu_if_->msgq_enqueue(gm);
331 usleep(5e5);
332 }
333 }
334
335 void
336 rx28_set_id()
337 {
338 init_rx28();
339
340 int old_id = argp_.parse_item_int(1);
341 int new_id = argp_.parse_item_int(2);
342
343 printf("Servo IDs *before* setting the ID:\n");
344 RobotisRX28::DeviceList devl = rx28_->discover();
345 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
346 printf(" %d\n", *i);
347 }
348
349 if (old_id < 0 || old_id >= RobotisRX28::BROADCAST_ID) {
350 printf("Invalid old ID %i, must be in range [0..%u]\n", old_id, RobotisRX28::BROADCAST_ID);
351 return;
352 }
353
354 if (new_id < 0 || new_id >= RobotisRX28::BROADCAST_ID) {
355 printf("Invalid new ID %i, must be in range [0..%u]\n", new_id, RobotisRX28::BROADCAST_ID);
356 return;
357 }
358
359 rx28_->set_id(old_id, new_id);
360
361 printf("Servo IDs *after* setting the ID:\n");
362 devl = rx28_->discover();
363 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
364 printf(" %d\n", *i);
365 }
366 }
367
368 void
369 rx28_discover()
370 {
371 init_rx28();
372
373 printf("Servo IDs on the bus:\n");
374 RobotisRX28::DeviceList devl = rx28_->discover();
375 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
376 printf(" %d\n", *i);
377 }
378 }
379
380private:
381 ArgumentParser argp_;
382 BlackBoard * bb_;
383 PanTiltInterface *ptu_if_;
384 float resolution_;
385 RobotisRX28 * rx28_;
386};
387
388/** Config tool main.
389 * @param argc argument count
390 * @param argv arguments
391 */
392int
393main(int argc, char **argv)
394{
395 try {
396 PTUJoystickControl ptuctrl(argc, argv);
397 ptuctrl.run();
398 } catch (Exception &e) {
399 printf("Running failed: %s\n\n", e.what());
400 print_usage(argv[0]);
401 exit(0);
402 }
403
404 return 0;
405}
Remote control PTUs via keyboard.
Definition: ffptu.cpp:68
PTUJoystickControl(int argc, char **argv)
Constructor.
Definition: ffptu.cpp:74
~PTUJoystickControl()
Destructor.
Definition: ffptu.cpp:88
void init_bb()
Initialize BB connection.
Definition: ffptu.cpp:99
void run()
Run control loop.
Definition: ffptu.cpp:161
void init_rx28()
Initialize Robotis RX28 raw servo access.
Definition: ffptu.cpp:149
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:44
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:47
void set_id(unsigned char id, unsigned char new_id)
Set ID.
Definition: rx28.cpp:1075
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: rx28.h:147
DeviceList discover(unsigned int total_timeout_ms=50)
Discover devices on the bus.
Definition: rx28.cpp:466
Parse command line arguments.
Definition: argparser.h:64
double parse_item_float(unsigned int index)
Parse item as double.
Definition: argparser.cpp:428
const std::vector< const char * > & items() const
Get non-option items.
Definition: argparser.cpp:447
const char * arg(const char *argn)
Get argument value.
Definition: argparser.cpp:177
bool parse_hostport(const char *argn, char **host, unsigned short int *port)
Parse host:port string.
Definition: argparser.cpp:224
std::vector< constchar * >::size_type num_items() const
Get number of non-option items.
Definition: argparser.cpp:456
long int parse_item_int(unsigned int index)
Parse item as integer.
Definition: argparser.cpp:405
bool has_arg(const char *argn)
Check if argument has been supplied.
Definition: argparser.cpp:165
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual InterfaceInfoList * list(const char *type_pattern, const char *id_pattern)=0
Get list of interfaces matching type and ID patterns.
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Interface information list.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
GotoMessage Fawkes BlackBoard Interface Message.
SetVelocityMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
float max_tilt_velocity() const
Get max_tilt_velocity value.
float pan() const
Get pan value.
float max_tilt() const
Get max_tilt value.
float max_pan() const
Get max_pan value.
float min_tilt() const
Get min_tilt value.
float tilt() const
Get tilt value.
float min_pan() const
Get min_pan value.
float max_pan_velocity() const
Get max_pan_velocity value.
Remote BlackBoard.
Definition: remote.h:50
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.
char getkey(int timeout_decisecs)
Get value of a single key-press non-blocking.
Definition: getkey.cpp:68