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 <core/exceptions/system.h>
26 #include <utils/system/argparser.h>
27 #include <logging/console.h>
28 #include <utils/system/getkey.h>
29 #include <utils/time/time.h>
30 #include <blackboard/remote.h>
31 #include <interface/interface_info.h>
32 
33 #include <cstdlib>
34 #include <cstdio>
35 #include <cstring>
36 #include <unistd.h>
37 
38 #include <interfaces/PanTiltInterface.h>
39 
40 using namespace fawkes;
41 
42 void
43 print_usage(const char *program_name)
44 {
45  printf("Usage: %s [-h] [-r host[:port]] <command>\n"
46  " Options:\n"
47  " -h This help message\n"
48  " -r host[:port] Remote host (and optionally port) to connect to\n"
49  " -p <ptu> PTU, interface must have id 'PanTilt <ptu>'\n"
50  " -l List available PTUs\n"
51  " -i Invert tilt control buttons\n\n"
52  " Commands:\n"
53  "move [pan P] [tilt T]\n"
54  " Move PTU, if either pan or tilt are supplied, send the\n"
55  " command and wait for angle to be reached. Otherwise enter\n"
56  " interactive mode\n"
57  "rx28-set-id <old-id> <new-id>\n"
58  " Send message to set a new servo ID.\n"
59  " WARNING: use this with only a single servo connected!\n"
60  "rx28-discover\n"
61  " Discover and print servos on the BUS.\n",
62  program_name);
63 }
64 
65 
66 /** Remote control PTUs via keyboard.
67  * @author Tim Niemueller
68  */
70 {
71  public:
72  /** Constructor.
73  * @param argc number of arguments in argv
74  * @param argv array of parameters passed into the program
75  */
76  PTUJoystickControl(int argc, char **argv)
77  : __argp(argc, argv, "hr:p:li")
78  {
79  __rx28 = NULL;
80  __bb = NULL;
81  __ptu_if = NULL;
82  __resolution = 0.1f;
83 
84  if ( __argp.has_arg("h") ) {
85  print_usage(argv[0]);
86  exit(0);
87  }
88  }
89 
90  /** Destructor. */
92  {
93  if (__bb) {
94  __bb->close(__ptu_if);
95  delete __bb;
96  }
97  delete __rx28;
98  }
99 
100 
101  /** Initialize BB connection. */
102  void init_bb()
103  {
104  char *host = (char *)"localhost";
105  unsigned short int port = 1910;
106  bool free_host = __argp.parse_hostport("r", &host, &port);
107 
108  __bb = new RemoteBlackBoard(host, port);
109  if (free_host) free(host);
110 
111  if (__argp.has_arg("l")) {
112  InterfaceInfoList *l = __bb->list("PanTiltInterface", "PanTilt *");
113  if (l->empty()) {
114  printf("No interfaces found");
115  }
116  for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
117  std::string id = i->id();
118  id = id.substr(std::string("PanTilt ").length());
119 
120  printf("PTU: %s Writer: %s\n", id.c_str(),
121  i->has_writer() ? "Yes" : "No");
122  }
123  delete l;
124  return;
125  }
126 
127  if (__argp.has_arg("p")) {
128  std::string iface_id = std::string("PanTilt ") + __argp.arg("p");
129  __ptu_if = __bb->open_for_reading<PanTiltInterface>(iface_id.c_str());
130  } else {
131  InterfaceInfoList *l = __bb->list("PanTiltInterface", "PanTilt *");
132  if (l->empty()) {
133  throw Exception("No PanTilt interface opened on remote!");
134  }
135  for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
136  if (i->has_writer()) {
137  __ptu_if = __bb->open_for_reading<PanTiltInterface>(i->id());
138  } else {
139  printf("Interface %s has no writer, ignoring\n", i->id());
140  }
141  }
142  delete l;
143  }
144 
145  if (!__ptu_if) {
146  throw Exception("No suitable PanTiltInterface found");
147  }
148 
149  }
150 
151  /** Initialize Robotis RX28 raw servo access. */
152  void init_rx28()
153  {
154  __rx28 = new RobotisRX28("/dev/ttyUSB0");
155  RobotisRX28::DeviceList devl = __rx28->discover();
156 
157  if (devl.empty()) {
158  throw Exception("No devices found\n");
159 
160  }
161 
162  }
163 
164 
165  /** Run control loop. */
166  void run()
167  {
168  if (__argp.num_items() == 0) {
169  interactive_move();
170  } else if (strcmp(__argp.items()[0], "move") == 0) {
171  if (__argp.num_items() == 1) {
172  interactive_move();
173  } else {
174  exec_move();
175  }
176  } else if (strcmp(__argp.items()[0], "rx28-set-id") == 0) {
177  rx28_set_id();
178 
179  } else if (strcmp(__argp.items()[0], "rx28-discover") == 0) {
180  rx28_discover();
181 
182  } else {
183  printf("Unknown command '%s'\n", __argp.items()[0]);
184  }
185  }
186 
187  private:
188 
189  void interactive_move()
190  {
191  init_bb();
192 
193  Time last, now;
194 
195  char tilt_up = 65;
196  char tilt_down = 66;
197  if (__argp.has_arg("i")) std::swap(tilt_up, tilt_down);
198 
199  float pan, tilt, new_pan, new_tilt;
200  float speed = 0.0, new_speed = 0.5;
201 
202  __ptu_if->read();
203  pan = new_pan = __ptu_if->pan();
204  tilt = new_tilt = __ptu_if->tilt();
205 
206  last.stamp();
207  char key = 0;
208  int wait_time = 5;
209  while (key != 'q') {
210  key = getkey(wait_time);
211  //printf("Key: %u = %u\n", key, key);
212  if (key != 0) {
213  now.stamp();
214  if ( (now - &last) < 0.5) {
215  wait_time = 1;
216  }
217  last.stamp();
218  }
219  if (key == 0) {
220  wait_time = 5;
221 
222  } else if (key == 27) {
223  key = getkey();
224  if (key == 0) {
225  // Escape key
226  new_pan = pan;
227  new_tilt = tilt;
228  } else {
229  if (key != 91) continue;
230 
231  key = getkey();
232  if (key == 0) 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 continue;
243 
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", pan_vel, tilt_vel,
266  __ptu_if->max_pan_velocity(), __ptu_if->max_tilt_velocity());
267 
269  new PanTiltInterface::SetVelocityMessage(pan_vel, tilt_vel);
270  __ptu_if->msgq_enqueue(svm);
271  }
272 
273  if ((pan != new_pan) || (tilt != new_tilt)) {
274  pan = new_pan;
275  tilt = new_tilt;
276 
277  printf("Goto %f/%f\n", pan, tilt);
278 
280  new PanTiltInterface::GotoMessage(pan, tilt);
281  __ptu_if->msgq_enqueue(gm);
282  }
283  }
284  }
285 
286 
287  void exec_move()
288  {
289  init_bb();
290 
291  float pan, tilt, new_pan, new_tilt;
292 
293  __ptu_if->read();
294  pan = new_pan = __ptu_if->pan();
295  tilt = new_tilt = __ptu_if->tilt();
296 
297  const std::vector< const char * > &items = __argp.items();
298  for (unsigned int i = 1; i < items.size(); ++i) {
299  if (strcmp(items[i], "pan") == 0) {
300  if (items.size() > i+1) {
301  new_pan = __argp.parse_item_float(++i);
302  } else {
303  printf("No pan value supplied, aborting.\n");
304  return;
305  }
306  } else if (strcmp(items[i], "tilt") == 0) {
307  if (items.size() > i+1) {
308  new_tilt = __argp.parse_item_float(++i);
309  } else {
310  printf("No tilt value supplied, aborting.\n");
311  return;
312  }
313  } else {
314  printf("Unknown parameter '%s', aborting.\n", items[i]);
315  return;
316  }
317  }
318 
319  if ((pan != new_pan) || (tilt != new_tilt)) {
320  printf("Goto pan %f and tilt %f\n", new_pan, new_tilt);
321 
323  new PanTiltInterface::SetVelocityMessage(__ptu_if->max_pan_velocity() / 2.,
324  __ptu_if->max_tilt_velocity() / 2.);
325  __ptu_if->msgq_enqueue(svm);
326 
328  new PanTiltInterface::GotoMessage(new_pan, new_tilt);
329  __ptu_if->msgq_enqueue(gm);
330  usleep(5e5);
331  }
332  }
333 
334 
335  void rx28_set_id()
336  {
337  init_rx28();
338 
339  int old_id = __argp.parse_item_int(1);
340  int new_id = __argp.parse_item_int(2);
341 
342  printf("Servo IDs *before* setting the ID:\n");
343  RobotisRX28::DeviceList devl = __rx28->discover();
344  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
345  printf(" %d\n", *i);
346  }
347 
348  if (old_id < 0 || old_id >= RobotisRX28::BROADCAST_ID) {
349  printf("Invalid old ID %i, must be in range [0..%u]\n",
350  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",
356  new_id, RobotisRX28::BROADCAST_ID);
357  return;
358  }
359 
360  __rx28->set_id(old_id, new_id);
361 
362  printf("Servo IDs *after* setting the ID:\n");
363  devl = __rx28->discover();
364  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
365  printf(" %d\n", *i);
366  }
367  }
368 
369  void 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 
380 
381  private:
382  ArgumentParser __argp;
383  BlackBoard *__bb;
384  PanTiltInterface *__ptu_if;
385  float __resolution;
386  RobotisRX28 *__rx28;
387 };
388 
389 
390 /** Config tool main.
391  * @param argc argument count
392  * @param argv arguments
393  */
394 int
395 main(int argc, char **argv)
396 {
397  try {
398  PTUJoystickControl ptuctrl(argc, argv);
399  ptuctrl.run();
400  } catch (Exception e) {
401  printf("Running failed: %s\n\n", e.what());
402  print_usage(argv[0]);
403  exit(0);
404  }
405 
406  return 0;
407 }
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:46
PTUJoystickControl(int argc, char **argv)
Constructor.
Definition: ffptu.cpp:76
Fawkes library namespace.
void run()
Run control loop.
Definition: ffptu.cpp:166
Remote control PTUs via keyboard.
Definition: ffptu.cpp:69
Parse command line arguments.
Definition: argparser.h:66
A class for handling time.
Definition: time.h:91
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
char getkey(int timeout_decisecs)
Get value of a single key-press non-blocking.
Definition: getkey.cpp:70
SetVelocityMessage Fawkes BlackBoard Interface Message.
Interface information list.
void init_bb()
Initialize BB connection.
Definition: ffptu.cpp:102
Base class for exceptions in Fawkes.
Definition: exception.h:36
GotoMessage Fawkes BlackBoard Interface Message.
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:42
Remote BlackBoard.
Definition: remote.h:48
PanTiltInterface Fawkes BlackBoard Interface.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: rx28.h:138
The BlackBoard abstract class.
Definition: blackboard.h:48
void init_rx28()
Initialize Robotis RX28 raw servo access.
Definition: ffptu.cpp:152
~PTUJoystickControl()
Destructor.
Definition: ffptu.cpp:91