23 #include "../robotis/rx28.h" 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> 38 #include <interfaces/PanTiltInterface.h> 43 print_usage(
const char *program_name)
45 printf(
"Usage: %s [-h] [-r host[:port]] <command>\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" 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" 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" 61 " Discover and print servos on the BUS.\n",
77 : __argp(argc, argv,
"hr:p:li")
84 if ( __argp.has_arg(
"h") ) {
94 __bb->close(__ptu_if);
104 char *host = (
char *)
"localhost";
105 unsigned short int port = 1910;
106 bool free_host = __argp.parse_hostport(
"r", &host, &port);
109 if (free_host) free(host);
111 if (__argp.has_arg(
"l")) {
114 printf(
"No interfaces found");
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());
120 printf(
"PTU: %s Writer: %s\n",
id.c_str(),
121 i->has_writer() ?
"Yes" :
"No");
127 if (__argp.has_arg(
"p")) {
128 std::string iface_id = std::string(
"PanTilt ") + __argp.arg(
"p");
133 throw Exception(
"No PanTilt interface opened on remote!");
135 for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
136 if (i->has_writer()) {
139 printf(
"Interface %s has no writer, ignoring\n", i->id());
146 throw Exception(
"No suitable PanTiltInterface found");
168 if (__argp.num_items() == 0) {
170 }
else if (strcmp(__argp.items()[0],
"move") == 0) {
171 if (__argp.num_items() == 1) {
176 }
else if (strcmp(__argp.items()[0],
"rx28-set-id") == 0) {
179 }
else if (strcmp(__argp.items()[0],
"rx28-discover") == 0) {
183 printf(
"Unknown command '%s'\n", __argp.items()[0]);
189 void interactive_move()
197 if (__argp.has_arg(
"i")) std::swap(tilt_up, tilt_down);
199 float pan, tilt, new_pan, new_tilt;
200 float speed = 0.0, new_speed = 0.5;
203 pan = new_pan = __ptu_if->pan();
204 tilt = new_tilt = __ptu_if->tilt();
214 if ( (now - &last) < 0.5) {
222 }
else if (key == 27) {
229 if (key != 91)
continue;
232 if (key == 0)
continue;
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') {
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);
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", pan_vel, tilt_vel,
266 __ptu_if->max_pan_velocity(), __ptu_if->max_tilt_velocity());
270 __ptu_if->msgq_enqueue(svm);
273 if ((pan != new_pan) || (tilt != new_tilt)) {
277 printf(
"Goto %f/%f\n", pan, tilt);
281 __ptu_if->msgq_enqueue(gm);
291 float pan, tilt, new_pan, new_tilt;
294 pan = new_pan = __ptu_if->pan();
295 tilt = new_tilt = __ptu_if->tilt();
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);
303 printf(
"No pan value supplied, aborting.\n");
306 }
else if (strcmp(items[i],
"tilt") == 0) {
307 if (items.size() > i+1) {
308 new_tilt = __argp.parse_item_float(++i);
310 printf(
"No tilt value supplied, aborting.\n");
314 printf(
"Unknown parameter '%s', aborting.\n", items[i]);
319 if ((pan != new_pan) || (tilt != new_tilt)) {
320 printf(
"Goto pan %f and tilt %f\n", new_pan, new_tilt);
324 __ptu_if->max_tilt_velocity() / 2.);
325 __ptu_if->msgq_enqueue(svm);
329 __ptu_if->msgq_enqueue(gm);
339 int old_id = __argp.parse_item_int(1);
340 int new_id = __argp.parse_item_int(2);
342 printf(
"Servo IDs *before* setting the ID:\n");
344 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
349 printf(
"Invalid old ID %i, must be in range [0..%u]\n",
355 printf(
"Invalid new ID %i, must be in range [0..%u]\n",
360 __rx28->set_id(old_id, new_id);
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) {
373 printf(
"Servo IDs on the bus:\n");
375 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
395 main(
int argc,
char **argv)
401 printf(
"Running failed: %s\n\n", e.
what());
402 print_usage(argv[0]);
std::list< unsigned char > DeviceList
List of servo IDs.
PTUJoystickControl(int argc, char **argv)
Constructor.
Fawkes library namespace.
void run()
Run control loop.
Remote control PTUs via keyboard.
Parse command line arguments.
A class for handling time.
virtual const char * what() const
Get primary string.
char getkey(int timeout_decisecs)
Get value of a single key-press non-blocking.
SetVelocityMessage Fawkes BlackBoard Interface Message.
Interface information list.
void init_bb()
Initialize BB connection.
Base class for exceptions in Fawkes.
GotoMessage Fawkes BlackBoard Interface Message.
Class to access a chain of Robotis RX28 servos.
PanTiltInterface Fawkes BlackBoard Interface.
Time & stamp()
Set this time to the current time.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
The BlackBoard abstract class.
void init_rx28()
Initialize Robotis RX28 raw servo access.
~PTUJoystickControl()
Destructor.