23 #include "../robotis/rx28.h"
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>
42 print_usage(
const char *program_name)
44 printf(
"Usage: %s [-h] [-r host[:port]] <command>\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"
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"
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"
60 " Discover and print servos on the BUS.\n",
81 if (argp_.has_arg(
"h")) {
101 char * host = (
char *)
"localhost";
102 unsigned short int port = 1910;
103 bool free_host = argp_.parse_hostport(
"r", &host, &port);
109 if (argp_.has_arg(
"l")) {
112 printf(
"No interfaces found");
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());
118 printf(
"PTU: %s Writer: %s\n",
id.c_str(), i->has_writer() ?
"Yes" :
"No");
124 if (argp_.has_arg(
"p")) {
125 std::string iface_id = std::string(
"PanTilt ") + argp_.arg(
"p");
130 throw Exception(
"No PanTilt interface opened on remote!");
132 for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
133 if (i->has_writer()) {
136 printf(
"Interface %s has no writer, ignoring\n", i->id());
143 throw Exception(
"No suitable PanTiltInterface found");
163 if (argp_.has_arg(
"l")) {
165 }
else if (argp_.num_items() == 0) {
167 }
else if (strcmp(argp_.items()[0],
"move") == 0) {
168 if (argp_.num_items() == 1) {
173 }
else if (strcmp(argp_.items()[0],
"rx28-set-id") == 0) {
176 }
else if (strcmp(argp_.items()[0],
"rx28-discover") == 0) {
180 printf(
"Unknown command '%s'\n", argp_.items()[0]);
194 if (argp_.has_arg(
"i"))
195 std::swap(tilt_up, tilt_down);
197 float pan, tilt, new_pan, new_tilt;
198 float speed = 0.0, new_speed = 0.5;
201 pan = new_pan = ptu_if_->pan();
202 tilt = new_tilt = ptu_if_->tilt();
212 if ((now - &last) < 0.5) {
220 }
else if (key == 27) {
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());
245 }
else if (key ==
'0') {
246 new_pan = new_tilt = 0.f;
247 }
else if (key ==
'9') {
249 new_tilt = M_PI / 2.;
250 }
else if (key ==
'r') {
252 }
else if (key ==
'R') {
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);
260 if (speed != new_speed) {
262 float pan_vel = speed * ptu_if_->max_pan_velocity();
263 float tilt_vel = speed * ptu_if_->max_tilt_velocity();
265 printf(
"Setting velocity %f/%f (max %f/%f)\n",
268 ptu_if_->max_pan_velocity(),
269 ptu_if_->max_tilt_velocity());
273 ptu_if_->msgq_enqueue(svm);
276 if ((pan != new_pan) || (tilt != new_tilt)) {
280 printf(
"Goto %f/%f\n", pan, tilt);
283 ptu_if_->msgq_enqueue(gm);
293 float pan, tilt, new_pan, new_tilt;
296 pan = new_pan = ptu_if_->pan();
297 tilt = new_tilt = ptu_if_->tilt();
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);
305 printf(
"No pan value supplied, aborting.\n");
308 }
else if (strcmp(items[i],
"tilt") == 0) {
309 if (items.size() > i + 1) {
310 new_tilt = argp_.parse_item_float(++i);
312 printf(
"No tilt value supplied, aborting.\n");
316 printf(
"Unknown parameter '%s', aborting.\n", items[i]);
321 if ((pan != new_pan) || (tilt != new_tilt)) {
322 printf(
"Goto pan %f and tilt %f\n", new_pan, new_tilt);
326 ptu_if_->max_tilt_velocity() / 2.);
327 ptu_if_->msgq_enqueue(svm);
330 ptu_if_->msgq_enqueue(gm);
340 int old_id = argp_.parse_item_int(1);
341 int new_id = argp_.parse_item_int(2);
343 printf(
"Servo IDs *before* setting the ID:\n");
345 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
359 rx28_->set_id(old_id, new_id);
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) {
373 printf(
"Servo IDs on the bus:\n");
375 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
393 main(
int argc,
char **argv)
399 printf(
"Running failed: %s\n\n", e.
what());
400 print_usage(argv[0]);
Remote control PTUs via keyboard.
PTUJoystickControl(int argc, char **argv)
Constructor.
~PTUJoystickControl()
Destructor.
void init_bb()
Initialize BB connection.
void run()
Run control loop.
void init_rx28()
Initialize Robotis RX28 raw servo access.
Class to access a chain of Robotis RX28 servos.
std::list< unsigned char > DeviceList
List of servo IDs.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Parse command line arguments.
The BlackBoard abstract class.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
Interface information list.
GotoMessage Fawkes BlackBoard Interface Message.
SetVelocityMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
A class for handling time.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.
char getkey(int timeout_decisecs)
Get value of a single key-press non-blocking.