Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * ffptu.cpp - Control PTU via keyboard 00004 * 00005 * Created: Thu Oct 06 16:28:16 2011 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "../robotis/rx28.h" 00024 00025 #include <core/exceptions/system.h> 00026 #include <utils/system/argparser.h> 00027 #include <logging/console.h> 00028 #include <utils/system/getkey.h> 00029 #include <utils/time/time.h> 00030 #include <blackboard/remote.h> 00031 #include <interface/interface_info.h> 00032 00033 #include <cstdlib> 00034 #include <cstdio> 00035 #include <cstring> 00036 #include <unistd.h> 00037 00038 #include <interfaces/PanTiltInterface.h> 00039 00040 using namespace fawkes; 00041 00042 void 00043 print_usage(const char *program_name) 00044 { 00045 printf("Usage: %s [-h] [-r host[:port]] <command>\n" 00046 " Options:\n" 00047 " -h This help message\n" 00048 " -r host[:port] Remote host (and optionally port) to connect to\n" 00049 " -p <ptu> PTU, interface must have id 'PanTilt <ptu>'\n" 00050 " -l List available PTUs\n" 00051 " -i Invert tilt control buttons\n\n" 00052 " Commands:\n" 00053 "move [pan P] [tilt T]\n" 00054 " Move PTU, if either pan or tilt are supplied, send the\n" 00055 " command and wait for angle to be reached. Otherwise enter\n" 00056 " interactive mode\n" 00057 "rx28-set-id <old-id> <new-id>\n" 00058 " Send message to set a new servo ID.\n" 00059 " WARNING: use this with only a single servo connected!\n" 00060 "rx28-discover\n" 00061 " Discover and print servos on the BUS.\n", 00062 program_name); 00063 } 00064 00065 00066 /** Remote control PTUs via keyboard. 00067 * @author Tim Niemueller 00068 */ 00069 class PTUJoystickControl 00070 { 00071 public: 00072 /** Constructor. 00073 * @param argc number of arguments in argv 00074 * @param argv array of parameters passed into the program 00075 */ 00076 PTUJoystickControl(int argc, char **argv) 00077 : __argp(argc, argv, "hr:p:li") 00078 { 00079 __rx28 = NULL; 00080 __bb = NULL; 00081 __ptu_if = NULL; 00082 __resolution = 0.1f; 00083 00084 if ( __argp.has_arg("h") ) { 00085 print_usage(argv[0]); 00086 exit(0); 00087 } 00088 } 00089 00090 /** Destructor. */ 00091 ~PTUJoystickControl() 00092 { 00093 if (__bb) { 00094 __bb->close(__ptu_if); 00095 delete __bb; 00096 } 00097 delete __rx28; 00098 } 00099 00100 00101 /** Initialize BB connection. */ 00102 void init_bb() 00103 { 00104 char *host = (char *)"localhost"; 00105 unsigned short int port = 1910; 00106 bool free_host = __argp.parse_hostport("r", &host, &port); 00107 00108 __bb = new RemoteBlackBoard(host, port); 00109 if (free_host) free(host); 00110 00111 if (__argp.has_arg("l")) { 00112 InterfaceInfoList *l = __bb->list("PanTiltInterface", "PanTilt *"); 00113 if (l->empty()) { 00114 printf("No interfaces found"); 00115 } 00116 for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) { 00117 std::string id = i->id(); 00118 id = id.substr(std::string("PanTilt ").length()); 00119 00120 printf("PTU: %s Writer: %s\n", id.c_str(), 00121 i->has_writer() ? "Yes" : "No"); 00122 } 00123 delete l; 00124 return; 00125 } 00126 00127 if (__argp.has_arg("p")) { 00128 std::string iface_id = std::string("PanTilt ") + __argp.arg("p"); 00129 __ptu_if = __bb->open_for_reading<PanTiltInterface>(iface_id.c_str()); 00130 } else { 00131 InterfaceInfoList *l = __bb->list("PanTiltInterface", "PanTilt *"); 00132 if (l->empty()) { 00133 throw Exception("No PanTilt interface opened on remote!"); 00134 } 00135 for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) { 00136 if (i->has_writer()) { 00137 __ptu_if = __bb->open_for_reading<PanTiltInterface>(i->id()); 00138 } else { 00139 printf("Interface %s has no writer, ignoring\n", i->id()); 00140 } 00141 } 00142 delete l; 00143 } 00144 00145 if (!__ptu_if) { 00146 throw Exception("No suitable PanTiltInterface found"); 00147 } 00148 00149 } 00150 00151 /** Initialize Robotis RX28 raw servo access. */ 00152 void init_rx28() 00153 { 00154 __rx28 = new RobotisRX28("/dev/ttyUSB0"); 00155 RobotisRX28::DeviceList devl = __rx28->discover(); 00156 00157 if (devl.empty()) { 00158 throw Exception("No devices found\n"); 00159 00160 } 00161 00162 } 00163 00164 00165 /** Run control loop. */ 00166 void run() 00167 { 00168 if (__argp.num_items() == 0) { 00169 interactive_move(); 00170 } else if (strcmp(__argp.items()[0], "move") == 0) { 00171 if (__argp.num_items() == 1) { 00172 interactive_move(); 00173 } else { 00174 exec_move(); 00175 } 00176 } else if (strcmp(__argp.items()[0], "rx28-set-id") == 0) { 00177 rx28_set_id(); 00178 00179 } else if (strcmp(__argp.items()[0], "rx28-discover") == 0) { 00180 rx28_discover(); 00181 00182 } else { 00183 printf("Unknown command '%s'\n", __argp.items()[0]); 00184 } 00185 } 00186 00187 private: 00188 00189 void interactive_move() 00190 { 00191 init_bb(); 00192 00193 Time last, now; 00194 00195 char tilt_up = 65; 00196 char tilt_down = 66; 00197 if (__argp.has_arg("i")) std::swap(tilt_up, tilt_down); 00198 00199 float pan, tilt, new_pan, new_tilt; 00200 float speed = 0.0, new_speed = 0.5; 00201 00202 __ptu_if->read(); 00203 pan = new_pan = __ptu_if->pan(); 00204 tilt = new_tilt = __ptu_if->tilt(); 00205 00206 last.stamp(); 00207 char key = 0; 00208 int wait_time = 5; 00209 while (key != 'q') { 00210 key = getkey(wait_time); 00211 //printf("Key: %u = %u\n", key, key); 00212 if (key != 0) { 00213 now.stamp(); 00214 if ( (now - &last) < 0.5) { 00215 wait_time = 1; 00216 } 00217 last.stamp(); 00218 } 00219 if (key == 0) { 00220 wait_time = 5; 00221 00222 } else if (key == 27) { 00223 key = getkey(); 00224 if (key == 0) { 00225 // Escape key 00226 new_pan = pan; 00227 new_tilt = tilt; 00228 } else { 00229 if (key != 91) continue; 00230 00231 key = getkey(); 00232 if (key == 0) continue; 00233 00234 if (key == tilt_up) { 00235 new_tilt = std::min(tilt + __resolution, __ptu_if->max_tilt()); 00236 } else if (key == tilt_down) { 00237 new_tilt = std::max(tilt - __resolution, __ptu_if->min_tilt()); 00238 } else if (key == 67) { 00239 new_pan = std::max(pan - __resolution, __ptu_if->min_pan()); 00240 } else if (key == 68) { 00241 new_pan = std::min(pan + __resolution, __ptu_if->max_pan()); 00242 } else continue; 00243 00244 } 00245 } else if (key == '0') { 00246 new_pan = new_tilt = 0.f; 00247 } else if (key == '9') { 00248 new_pan = 0; 00249 new_tilt = M_PI / 2.; 00250 } else if (key == 'r') { 00251 __resolution = 0.1f; 00252 } else if (key == 'R') { 00253 __resolution = 0.01f; 00254 } else if (key == '+') { 00255 new_speed = std::min(speed + 0.1, 1.0); 00256 } else if (key == '-') { 00257 new_speed = std::max(speed - 0.1, 0.0); 00258 } 00259 00260 if (speed != new_speed) { 00261 speed = new_speed; 00262 float pan_vel = speed * __ptu_if->max_pan_velocity(); 00263 float tilt_vel = speed * __ptu_if->max_tilt_velocity(); 00264 00265 printf("Setting velocity %f/%f (max %f/%f)\n", pan_vel, tilt_vel, 00266 __ptu_if->max_pan_velocity(), __ptu_if->max_tilt_velocity()); 00267 00268 PanTiltInterface::SetVelocityMessage *svm = 00269 new PanTiltInterface::SetVelocityMessage(pan_vel, tilt_vel); 00270 __ptu_if->msgq_enqueue(svm); 00271 } 00272 00273 if ((pan != new_pan) || (tilt != new_tilt)) { 00274 pan = new_pan; 00275 tilt = new_tilt; 00276 00277 printf("Goto %f/%f\n", pan, tilt); 00278 00279 PanTiltInterface::GotoMessage *gm = 00280 new PanTiltInterface::GotoMessage(pan, tilt); 00281 __ptu_if->msgq_enqueue(gm); 00282 } 00283 } 00284 } 00285 00286 00287 void exec_move() 00288 { 00289 init_bb(); 00290 00291 float pan, tilt, new_pan, new_tilt; 00292 00293 __ptu_if->read(); 00294 pan = new_pan = __ptu_if->pan(); 00295 tilt = new_tilt = __ptu_if->tilt(); 00296 00297 const std::vector< const char * > &items = __argp.items(); 00298 for (unsigned int i = 1; i < items.size(); ++i) { 00299 if (strcmp(items[i], "pan") == 0) { 00300 if (items.size() > i+1) { 00301 new_pan = __argp.parse_item_float(++i); 00302 } else { 00303 printf("No pan value supplied, aborting.\n"); 00304 return; 00305 } 00306 } else if (strcmp(items[i], "tilt") == 0) { 00307 if (items.size() > i+1) { 00308 new_tilt = __argp.parse_item_float(++i); 00309 } else { 00310 printf("No tilt value supplied, aborting.\n"); 00311 return; 00312 } 00313 } else { 00314 printf("Unknown parameter '%s', aborting.\n", items[i]); 00315 return; 00316 } 00317 } 00318 00319 if ((pan != new_pan) || (tilt != new_tilt)) { 00320 printf("Goto pan %f and tilt %f\n", new_pan, new_tilt); 00321 00322 PanTiltInterface::SetVelocityMessage *svm = 00323 new PanTiltInterface::SetVelocityMessage(__ptu_if->max_pan_velocity() / 2., 00324 __ptu_if->max_tilt_velocity() / 2.); 00325 __ptu_if->msgq_enqueue(svm); 00326 00327 PanTiltInterface::GotoMessage *gm = 00328 new PanTiltInterface::GotoMessage(new_pan, new_tilt); 00329 __ptu_if->msgq_enqueue(gm); 00330 usleep(5e5); 00331 } 00332 } 00333 00334 00335 void rx28_set_id() 00336 { 00337 init_rx28(); 00338 00339 int old_id = __argp.parse_item_int(1); 00340 int new_id = __argp.parse_item_int(2); 00341 00342 printf("Servo IDs *before* setting the ID:\n"); 00343 RobotisRX28::DeviceList devl = __rx28->discover(); 00344 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00345 printf(" %d\n", *i); 00346 } 00347 00348 if (old_id < 0 || old_id >= RobotisRX28::BROADCAST_ID) { 00349 printf("Invalid old ID %i, must be in range [0..%u]\n", 00350 old_id, RobotisRX28::BROADCAST_ID); 00351 return; 00352 } 00353 00354 if (new_id < 0 || new_id >= RobotisRX28::BROADCAST_ID) { 00355 printf("Invalid new ID %i, must be in range [0..%u]\n", 00356 new_id, RobotisRX28::BROADCAST_ID); 00357 return; 00358 } 00359 00360 __rx28->set_id(old_id, new_id); 00361 00362 printf("Servo IDs *after* setting the ID:\n"); 00363 devl = __rx28->discover(); 00364 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00365 printf(" %d\n", *i); 00366 } 00367 } 00368 00369 void rx28_discover() 00370 { 00371 init_rx28(); 00372 00373 printf("Servo IDs on the bus:\n"); 00374 RobotisRX28::DeviceList devl = __rx28->discover(); 00375 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00376 printf(" %d\n", *i); 00377 } 00378 } 00379 00380 00381 private: 00382 ArgumentParser __argp; 00383 BlackBoard *__bb; 00384 PanTiltInterface *__ptu_if; 00385 float __resolution; 00386 RobotisRX28 *__rx28; 00387 }; 00388 00389 00390 /** Config tool main. 00391 * @param argc argument count 00392 * @param argv arguments 00393 */ 00394 int 00395 main(int argc, char **argv) 00396 { 00397 try { 00398 PTUJoystickControl ptuctrl(argc, argv); 00399 ptuctrl.run(); 00400 } catch (Exception e) { 00401 printf("Running failed: %s\n\n", e.what()); 00402 print_usage(argv[0]); 00403 exit(0); 00404 } 00405 00406 return 0; 00407 }