Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rx28_thread.h - RX28 pan/tilt unit act thread 00004 * 00005 * Created: Thu Jun 18 09:53:49 2009 00006 * Copyright 2006-2009 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 "rx28_thread.h" 00024 #include "rx28.h" 00025 00026 #include <core/threading/mutex_locker.h> 00027 #include <interfaces/PanTiltInterface.h> 00028 #include <interfaces/LedInterface.h> 00029 00030 #include <cstdarg> 00031 #include <cmath> 00032 00033 using namespace fawkes; 00034 00035 /** @class PanTiltRX28Thread "rx28_thread.h" 00036 * PanTilt act thread for RX28 PTU. 00037 * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and 00038 * interacts with the controller of the RX28 PTU. 00039 * @author Tim Niemueller 00040 */ 00041 00042 /** Constructor. 00043 * @param pantilt_cfg_prefix pantilt plugin configuration prefix 00044 * @param ptu_cfg_prefix configuration prefix specific for the PTU 00045 * @param ptu_name name of the PTU configuration 00046 */ 00047 PanTiltRX28Thread::PanTiltRX28Thread(std::string &pantilt_cfg_prefix, 00048 std::string &ptu_cfg_prefix, 00049 std::string &ptu_name) 00050 : PanTiltActThread("PanTiltRX28Thread"), 00051 BlackBoardInterfaceListener("PanTiltRX28Thread") 00052 { 00053 set_name("PanTiltRX28Thread(%s)", ptu_name.c_str()); 00054 00055 __pantilt_cfg_prefix = pantilt_cfg_prefix; 00056 __ptu_cfg_prefix = ptu_cfg_prefix; 00057 __ptu_name = ptu_name; 00058 00059 __rx28 = NULL; 00060 } 00061 00062 00063 void 00064 PanTiltRX28Thread::init() 00065 { 00066 // Note: due to the use of auto_ptr and RefPtr resources are automatically 00067 // freed on destruction, therefore no special handling is necessary in init() 00068 // itself! 00069 00070 __cfg_device = config->get_string((__ptu_cfg_prefix + "device").c_str()); 00071 __cfg_read_timeout_ms = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str()); 00072 __cfg_disc_timeout_ms = config->get_uint((__ptu_cfg_prefix + "discover_timeout_ms").c_str()); 00073 __cfg_pan_servo_id = config->get_uint((__ptu_cfg_prefix + "pan_servo_id").c_str()); 00074 __cfg_tilt_servo_id = config->get_uint((__ptu_cfg_prefix + "tilt_servo_id").c_str()); 00075 __cfg_pan_zero_offset = config->get_int((__ptu_cfg_prefix + "pan_zero_offset").c_str()); 00076 __cfg_tilt_zero_offset = config->get_int((__ptu_cfg_prefix + "tilt_zero_offset").c_str()); 00077 __cfg_goto_zero_start = config->get_bool((__ptu_cfg_prefix + "goto_zero_start").c_str()); 00078 __cfg_turn_off = config->get_bool((__ptu_cfg_prefix + "turn_off").c_str()); 00079 __cfg_cw_compl_margin = config->get_uint((__ptu_cfg_prefix + "cw_compl_margin").c_str()); 00080 __cfg_ccw_compl_margin = config->get_uint((__ptu_cfg_prefix + "ccw_compl_margin").c_str()); 00081 __cfg_cw_compl_slope = config->get_uint((__ptu_cfg_prefix + "cw_compl_slope").c_str()); 00082 __cfg_ccw_compl_slope = config->get_uint((__ptu_cfg_prefix + "ccw_compl_slope").c_str()); 00083 __cfg_pan_min = config->get_float((__ptu_cfg_prefix + "pan_min").c_str()); 00084 __cfg_pan_max = config->get_float((__ptu_cfg_prefix + "pan_max").c_str()); 00085 __cfg_tilt_min = config->get_float((__ptu_cfg_prefix + "tilt_min").c_str()); 00086 __cfg_tilt_max = config->get_float((__ptu_cfg_prefix + "tilt_max").c_str()); 00087 __cfg_pan_margin = config->get_float((__ptu_cfg_prefix + "pan_margin").c_str()); 00088 __cfg_tilt_margin = config->get_float((__ptu_cfg_prefix + "tilt_margin").c_str()); 00089 00090 bool pan_servo_found = false, tilt_servo_found = false; 00091 00092 __rx28 = new RobotisRX28(__cfg_device.c_str(), __cfg_read_timeout_ms); 00093 RobotisRX28::DeviceList devl = __rx28->discover(); 00094 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00095 if (__cfg_pan_servo_id == *i) { 00096 pan_servo_found = true; 00097 } else if (__cfg_tilt_servo_id == *i) { 00098 tilt_servo_found = true; 00099 } else { 00100 logger->log_warn(name(), "Servo %u in PTU servo chain, but neither " 00101 "configured as pan nor as tilt servo", *i); 00102 } 00103 } 00104 00105 // We only want responses to be sent on explicit READ to speed up communication 00106 __rx28->set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ); 00107 // set compliance values 00108 __rx28->set_compliance_values(RobotisRX28::BROADCAST_ID, 00109 __cfg_cw_compl_margin, __cfg_cw_compl_slope, 00110 __cfg_ccw_compl_margin, __cfg_ccw_compl_slope); 00111 __rx28->set_led_enabled(__cfg_pan_servo_id, false); 00112 00113 00114 if (! (pan_servo_found && tilt_servo_found)) { 00115 throw Exception("Pan and/or tilt servo not found: pan: %i tilt: %i", 00116 pan_servo_found, tilt_servo_found); 00117 } 00118 00119 // If you have more than one interface: catch exception and close them! 00120 std::string bbid = "PanTilt " + __ptu_name; 00121 __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str()); 00122 __pantilt_if->set_calibrated(true); 00123 __pantilt_if->set_min_pan(__cfg_pan_min); 00124 __pantilt_if->set_max_pan(__cfg_pan_max); 00125 __pantilt_if->set_min_tilt(__cfg_tilt_min); 00126 __pantilt_if->set_max_tilt(__cfg_tilt_max); 00127 __pantilt_if->set_pan_margin(__cfg_pan_margin); 00128 __pantilt_if->set_tilt_margin(__cfg_tilt_margin); 00129 __pantilt_if->set_max_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id)); 00130 __pantilt_if->set_max_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id)); 00131 __pantilt_if->set_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id)); 00132 __pantilt_if->set_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id)); 00133 __pantilt_if->write(); 00134 00135 __led_if = blackboard->open_for_writing<LedInterface>(bbid.c_str()); 00136 00137 __wt = new WorkerThread(__ptu_name, logger, __rx28, 00138 __cfg_pan_servo_id, __cfg_tilt_servo_id, 00139 __cfg_pan_min, __cfg_pan_max, __cfg_tilt_min, __cfg_tilt_max, 00140 __cfg_pan_zero_offset, __cfg_tilt_zero_offset); 00141 __wt->set_margins(__cfg_pan_margin, __cfg_tilt_margin); 00142 __wt->start(); 00143 __wt->set_enabled(true); 00144 if ( __cfg_goto_zero_start ) { 00145 __wt->goto_pantilt(0, 0); 00146 } 00147 00148 bbil_add_message_interface(__pantilt_if); 00149 blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES); 00150 00151 #ifdef USE_TIMETRACKER 00152 __tt.reset(new TimeTracker()); 00153 __tt_count = 0; 00154 __ttc_read_sensor = __tt->add_class("Read Sensor"); 00155 #endif 00156 00157 } 00158 00159 00160 void 00161 PanTiltRX28Thread::finalize() 00162 { 00163 logger->log_debug(name(), "Finalizing"); 00164 blackboard->unregister_listener(this); 00165 blackboard->close(__pantilt_if); 00166 blackboard->close(__led_if); 00167 00168 if (__cfg_turn_off) { 00169 try { 00170 __rx28->set_led_enabled(__cfg_pan_servo_id, false); 00171 __rx28->set_led_enabled(__cfg_tilt_servo_id, false); 00172 __rx28->set_torques_enabled(false, 2, __cfg_pan_servo_id, __cfg_tilt_servo_id); 00173 } catch (Exception &e) { 00174 // ignored 00175 } 00176 } 00177 00178 __wt->cancel(); 00179 __wt->join(); 00180 delete __wt; 00181 00182 // Setting to NULL deletes instance (RefPtr) 00183 __rx28 = NULL; 00184 } 00185 00186 00187 /** Update sensor values as necessary. 00188 * To be called only from PanTiltSensorThread. Writes the current pan/tilt 00189 * data into the interface. 00190 */ 00191 void 00192 PanTiltRX28Thread::update_sensor_values() 00193 { 00194 if (__wt->has_fresh_data()) { 00195 float pan = 0, tilt = 0, panvel=0, tiltvel=0; 00196 __wt->get_pantilt(pan, tilt); 00197 __wt->get_velocities(panvel, tiltvel); 00198 __pantilt_if->set_pan(pan); 00199 __pantilt_if->set_tilt(tilt); 00200 __pantilt_if->set_pan_velocity(panvel); 00201 __pantilt_if->set_tilt_velocity(tiltvel); 00202 __pantilt_if->set_enabled(__wt->is_enabled()); 00203 __pantilt_if->set_final(__wt->is_final()); 00204 __pantilt_if->write(); 00205 } 00206 } 00207 00208 00209 void 00210 PanTiltRX28Thread::loop() 00211 { 00212 __pantilt_if->set_final(__wt->is_final()); 00213 00214 while (! __pantilt_if->msgq_empty() ) { 00215 if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) { 00216 // ignored 00217 00218 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) { 00219 PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg); 00220 00221 __wt->goto_pantilt(msg->pan(), msg->tilt()); 00222 __pantilt_if->set_msgid(msg->id()); 00223 __pantilt_if->set_final(false); 00224 00225 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::TimedGotoMessage>()) { 00226 PanTiltInterface::TimedGotoMessage *msg = __pantilt_if->msgq_first(msg); 00227 00228 __wt->goto_pantilt_timed(msg->pan(), msg->tilt(), msg->time_sec()); 00229 __pantilt_if->set_msgid(msg->id()); 00230 __pantilt_if->set_final(false); 00231 00232 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) { 00233 PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg); 00234 00235 __wt->goto_pantilt(0, 0); 00236 __pantilt_if->set_msgid(msg->id()); 00237 __pantilt_if->set_final(false); 00238 00239 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) { 00240 PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg); 00241 00242 __wt->set_enabled(msg->is_enabled()); 00243 00244 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) { 00245 PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg); 00246 00247 if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) { 00248 logger->log_warn(name(), "Desired pan velocity %f too high, max is %f", 00249 msg->pan_velocity(), __pantilt_if->max_pan_velocity()); 00250 } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) { 00251 logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f", 00252 msg->tilt_velocity(), __pantilt_if->max_tilt_velocity()); 00253 } else { 00254 __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity()); 00255 } 00256 00257 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetMarginMessage>()) { 00258 PanTiltInterface::SetMarginMessage *msg = __pantilt_if->msgq_first(msg); 00259 00260 __wt->set_margins(msg->pan_margin(), msg->tilt_margin()); 00261 __pantilt_if->set_pan_margin(msg->pan_margin()); 00262 __pantilt_if->set_tilt_margin(msg->tilt_margin()); 00263 00264 } else { 00265 logger->log_warn(name(), "Unknown message received"); 00266 } 00267 00268 __pantilt_if->msgq_pop(); 00269 } 00270 00271 __pantilt_if->write(); 00272 00273 bool write_led_if = false; 00274 while (! __led_if->msgq_empty() ) { 00275 write_led_if = true; 00276 if (__led_if->msgq_first_is<LedInterface::SetIntensityMessage>()) { 00277 LedInterface::SetIntensityMessage *msg = __led_if->msgq_first(msg); 00278 __wt->set_led_enabled((msg->intensity() >= 0.5)); 00279 __led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF); 00280 } else if (__led_if->msgq_first_is<LedInterface::TurnOnMessage>()) { 00281 __wt->set_led_enabled(true); 00282 __led_if->set_intensity(LedInterface::ON); 00283 } else if (__led_if->msgq_first_is<LedInterface::TurnOffMessage>()) { 00284 __wt->set_led_enabled(false); 00285 __led_if->set_intensity(LedInterface::OFF); 00286 } 00287 00288 __led_if->msgq_pop(); 00289 } 00290 if (write_led_if) __led_if->write(); 00291 00292 } 00293 00294 00295 bool 00296 PanTiltRX28Thread::bb_interface_message_received(Interface *interface, 00297 Message *message) throw() 00298 { 00299 if (message->is_of_type<PanTiltInterface::StopMessage>()) { 00300 __wt->stop_motion(); 00301 return false; // do not enqueue StopMessage 00302 } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) { 00303 __wt->stop_motion(); 00304 logger->log_info(name(), "Flushing message queue"); 00305 __pantilt_if->msgq_flush(); 00306 return false; 00307 } else { 00308 logger->log_info(name(), "Received message of type %s, enqueueing", message->type()); 00309 return true; 00310 } 00311 } 00312 00313 00314 /** @class PanTiltRX28Thread::WorkerThread "robotis/rx28_thread.h" 00315 * Worker thread for the PanTiltRX28Thread. 00316 * This continuous thread issues commands to the RX28 chain. In each loop it 00317 * will first execute pending operations, and then update the sensor data (lengthy 00318 * operation). Sensor data will only be updated while either a servo in the chain 00319 * is still moving or torque is disabled (so the motor can be move manually). 00320 * @author Tim Niemueller 00321 */ 00322 00323 00324 /** Constructor. 00325 * @param ptu_name name of the pan/tilt unit 00326 * @param logger logger 00327 * @param rx28 RX28 chain 00328 * @param pan_servo_id servo ID of the pan servo 00329 * @param tilt_servo_id servo ID of the tilt servo 00330 * @param pan_min minimum pan in rad 00331 * @param pan_min maximum pan in rad 00332 * @param tilt_min minimum tilt in rad 00333 * @param tilt_max maximum tilt in rad 00334 * @param pan_zero_offset pan offset from zero in servo ticks 00335 * @param tilt_zero_offset tilt offset from zero in servo ticks 00336 */ 00337 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name, 00338 fawkes::Logger *logger, 00339 fawkes::RefPtr<RobotisRX28> rx28, 00340 unsigned char pan_servo_id, 00341 unsigned char tilt_servo_id, 00342 float &pan_min, float &pan_max, 00343 float &tilt_min, float &tilt_max, 00344 int &pan_zero_offset, 00345 int &tilt_zero_offset) 00346 : Thread("", Thread::OPMODE_WAITFORWAKEUP) 00347 { 00348 set_name("RX28WorkerThread(%s)", ptu_name.c_str()); 00349 set_coalesce_wakeups(true); 00350 00351 __logger = logger; 00352 00353 __value_mutex = new Mutex(); 00354 00355 __rx28 = rx28; 00356 __move_pending = false; 00357 __target_pan = 0; 00358 __target_tilt = 0; 00359 __pan_servo_id = pan_servo_id; 00360 __tilt_servo_id = tilt_servo_id; 00361 00362 __pan_min = pan_min; 00363 __pan_max = pan_max; 00364 __tilt_min = tilt_min; 00365 __tilt_max = tilt_max; 00366 __pan_zero_offset = pan_zero_offset; 00367 __tilt_zero_offset = tilt_zero_offset; 00368 __enable = false; 00369 __disable = false; 00370 __led_enable = false; 00371 __led_disable = false; 00372 00373 __max_pan_speed = __rx28->get_max_supported_speed(__pan_servo_id); 00374 __max_tilt_speed = __rx28->get_max_supported_speed(__tilt_servo_id); 00375 } 00376 00377 00378 /** Destructor. */ 00379 PanTiltRX28Thread::WorkerThread::~WorkerThread() 00380 { 00381 delete __value_mutex; 00382 } 00383 00384 00385 /** Enable or disable servo. 00386 * @param enabled true to enable servos, false to turn them off 00387 */ 00388 void 00389 PanTiltRX28Thread::WorkerThread::set_enabled(bool enabled) 00390 { 00391 MutexLocker lock(__value_mutex); 00392 if (enabled) { 00393 __enable = true; 00394 __disable = false; 00395 } else { 00396 __enable = false; 00397 __disable = true; 00398 } 00399 wakeup(); 00400 } 00401 00402 00403 /** Enable or disable LED. 00404 * @param enabled true to enable LED, false to turn it off 00405 */ 00406 void 00407 PanTiltRX28Thread::WorkerThread::set_led_enabled(bool enabled) 00408 { 00409 MutexLocker lock(__value_mutex); 00410 if (enabled) { 00411 __led_enable = true; 00412 __led_disable = false; 00413 } else { 00414 __led_enable = false; 00415 __led_disable = true; 00416 } 00417 wakeup(); 00418 } 00419 00420 00421 /** Stop currently running motion. */ 00422 void 00423 PanTiltRX28Thread::WorkerThread::stop_motion() 00424 { 00425 float pan = 0, tilt = 0; 00426 get_pantilt(pan, tilt); 00427 goto_pantilt(pan, tilt); 00428 } 00429 00430 00431 /** Goto desired pan/tilt values. 00432 * @param pan pan in radians 00433 * @param tilt tilt in radians 00434 */ 00435 void 00436 PanTiltRX28Thread::WorkerThread::goto_pantilt(float pan, float tilt) 00437 { 00438 MutexLocker lock(__value_mutex); 00439 __target_pan = pan; 00440 __target_tilt = tilt; 00441 __move_pending = true; 00442 wakeup(); 00443 } 00444 00445 00446 /** Goto desired pan/tilt values in a specified time. 00447 * @param pan pan in radians 00448 * @param tilt tilt in radians 00449 */ 00450 void 00451 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(float pan, float tilt, float time_sec) 00452 { 00453 MutexLocker lock(__value_mutex); 00454 __target_pan = pan; 00455 __target_tilt = tilt; 00456 __move_pending = true; 00457 00458 float cpan=0, ctilt=0; 00459 get_pantilt(cpan, ctilt); 00460 00461 float pan_diff = fabs(pan - cpan); 00462 float tilt_diff = fabs(tilt - ctilt); 00463 00464 float req_pan_vel = pan_diff / time_sec; 00465 float req_tilt_vel = tilt_diff / time_sec; 00466 00467 __logger->log_debug(name(), "Current: %f/%f Des: %f/%f Time: %f Diff: %f/%f ReqVel: %f/%f", 00468 cpan, ctilt, pan, tilt, time_sec, pan_diff, tilt_diff, req_pan_vel, req_tilt_vel); 00469 00470 00471 if (req_pan_vel > __max_pan_speed) { 00472 __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a " 00473 "pan speed of %f rad/s, which is greater than the maximum " 00474 "of %f rad/s, reducing to max", pan, tilt, time_sec, 00475 req_pan_vel, __max_pan_speed); 00476 req_pan_vel = __max_pan_speed; 00477 } 00478 00479 if (req_tilt_vel > __max_tilt_speed) { 00480 __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a " 00481 "tilt speed of %f rad/s, which is greater than the maximum of " 00482 "%f rad/s, reducing to max", pan, tilt, time_sec, 00483 req_tilt_vel, __max_tilt_speed); 00484 req_tilt_vel = __max_tilt_speed; 00485 } 00486 00487 lock.unlock(); 00488 set_velocities(req_pan_vel, req_tilt_vel); 00489 00490 wakeup(); 00491 } 00492 00493 00494 /** Set desired velocities. 00495 * @param pan_vel pan velocity 00496 * @param tilt_vel tilt velocity 00497 */ 00498 void 00499 PanTiltRX28Thread::WorkerThread::set_velocities(float pan_vel, float tilt_vel) 00500 { 00501 MutexLocker lock(__value_mutex); 00502 float pan_tmp = roundf((pan_vel / __max_pan_speed) * RobotisRX28::MAX_SPEED); 00503 float tilt_tmp = roundf((tilt_vel / __max_tilt_speed) * RobotisRX28::MAX_SPEED); 00504 00505 __logger->log_debug(name(), "old speed: %u/%u new speed: %f/%f", __pan_vel, 00506 __tilt_vel, pan_tmp, tilt_tmp); 00507 00508 if ((pan_tmp >= 0) && (pan_tmp <= RobotisRX28::MAX_SPEED)) { 00509 __pan_vel = (unsigned int)pan_tmp; 00510 __velo_pending = true; 00511 } else { 00512 __logger->log_warn(name(), "Calculated pan value out of bounds, min: 0 max: %u des: %u", 00513 RobotisRX28::MAX_SPEED, (unsigned int)pan_tmp); 00514 } 00515 00516 if ((tilt_tmp >= 0) && (tilt_tmp <= RobotisRX28::MAX_SPEED)) { 00517 __tilt_vel = (unsigned int)tilt_tmp; 00518 __velo_pending = true; 00519 } else { 00520 __logger->log_warn(name(), "Calculated tilt value out of bounds, min: 0 max: %u des: %u", 00521 RobotisRX28::MAX_SPEED, (unsigned int)tilt_tmp); 00522 } 00523 } 00524 00525 00526 /** Get current velocities. 00527 * @param pan_vel upon return contains current pan velocity 00528 * @param tilt_vel upon return contains current tilt velocity 00529 */ 00530 void 00531 PanTiltRX28Thread::WorkerThread::get_velocities(float &pan_vel, float &tilt_vel) 00532 { 00533 unsigned int pan_velticks = __rx28->get_goal_speed(__pan_servo_id); 00534 unsigned int tilt_velticks = __rx28->get_goal_speed(__tilt_servo_id); 00535 00536 pan_velticks = (unsigned int)(((float)pan_velticks / (float)RobotisRX28::MAX_SPEED) * __max_pan_speed); 00537 tilt_velticks = (unsigned int)(((float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * __max_tilt_speed); 00538 } 00539 00540 00541 /** Set desired velocities. 00542 * @param pan_vel pan velocity 00543 * @param tilt_vel tilt velocity 00544 */ 00545 void 00546 PanTiltRX28Thread::WorkerThread::set_margins(float pan_margin, float tilt_margin) 00547 { 00548 if (pan_margin > 0.0) __pan_margin = pan_margin; 00549 if (tilt_margin > 0.0) __tilt_margin = tilt_margin; 00550 //__logger->log_warn(name(), "Margins set to %f, %f", __pan_margin, __tilt_margin); 00551 } 00552 00553 00554 /** Get pan/tilt value. 00555 * @param pan upon return contains the current pan value 00556 * @param tilt upon return contains the current tilt value 00557 */ 00558 void 00559 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt) 00560 { 00561 int pan_ticks = ((int)__rx28->get_position(__pan_servo_id) - __pan_zero_offset - RobotisRX28::CENTER_POSITION); 00562 int tilt_ticks = ((int)__rx28->get_position(__tilt_servo_id) - (int)__tilt_zero_offset - (int)RobotisRX28::CENTER_POSITION); 00563 00564 pan = pan_ticks * RobotisRX28::RAD_PER_POS_TICK; 00565 tilt = tilt_ticks * RobotisRX28::RAD_PER_POS_TICK; 00566 } 00567 00568 00569 /** Check if motion is final. 00570 * @return true if motion is final, false otherwise 00571 */ 00572 bool 00573 PanTiltRX28Thread::WorkerThread::is_final() 00574 { 00575 MutexLocker lock(__value_mutex); 00576 float pan, tilt; 00577 get_pantilt(pan, tilt); 00578 //__logger->log_debug(name(), "P: %f T: %f TP: %f TT: %f PM: %f TM: %f", 00579 // pan, tilt, __target_pan, __target_tilt, __pan_margin, __tilt_margin); 00580 return ( (fabs(pan - __target_pan) <= __pan_margin) && 00581 (fabs(tilt - __target_tilt) <= __tilt_margin) ) || 00582 (! __rx28->is_moving(__pan_servo_id) && 00583 ! __rx28->is_moving(__tilt_servo_id)); 00584 } 00585 00586 00587 /** Check if PTU is enabled. 00588 * @return true if torque is enabled for both servos, false otherwise 00589 */ 00590 bool 00591 PanTiltRX28Thread::WorkerThread::is_enabled() 00592 { 00593 MutexLocker lock(__value_mutex); 00594 return (__rx28->is_torque_enabled(__pan_servo_id) && 00595 __rx28->is_torque_enabled(__tilt_servo_id)); 00596 } 00597 00598 00599 /** Check is fresh sensor data is available. 00600 * Note that this method will return true at once per sensor update cycle. 00601 * @return true if fresh data is available, false otherwise 00602 */ 00603 bool 00604 PanTiltRX28Thread::WorkerThread::has_fresh_data() 00605 { 00606 bool rv = __fresh_data; 00607 __fresh_data = false; 00608 return rv; 00609 } 00610 00611 00612 void 00613 PanTiltRX28Thread::WorkerThread::loop() 00614 { 00615 if (__enable) { 00616 __value_mutex->lock(); 00617 __enable = false; 00618 __value_mutex->unlock(); 00619 __rx28->set_led_enabled(__tilt_servo_id, true); 00620 __rx28->set_torques_enabled(true, 2, __pan_servo_id, __tilt_servo_id); 00621 } else if (__disable) { 00622 __value_mutex->lock(); 00623 __disable = false; 00624 __value_mutex->unlock(); 00625 __rx28->set_led_enabled(__tilt_servo_id, false); 00626 __rx28->set_torques_enabled(false, 2, __pan_servo_id, __tilt_servo_id); 00627 if (__led_enable || __led_disable || __velo_pending || __move_pending) usleep(3000); 00628 } 00629 00630 if (__led_enable) { 00631 __value_mutex->lock(); 00632 __led_enable = false; 00633 __value_mutex->unlock(); 00634 __rx28->set_led_enabled(__pan_servo_id, true); 00635 if (__velo_pending || __move_pending) usleep(3000); 00636 } else if (__led_disable) { 00637 __value_mutex->lock(); 00638 __led_disable = false; 00639 __value_mutex->unlock(); 00640 __rx28->set_led_enabled(__pan_servo_id, false); 00641 if (__velo_pending || __move_pending) usleep(3000); 00642 } 00643 00644 if (__velo_pending) { 00645 __value_mutex->lock(); 00646 __velo_pending = false; 00647 unsigned int pan_vel = __pan_vel; 00648 unsigned int tilt_vel = __tilt_vel; 00649 __value_mutex->unlock(); 00650 __rx28->set_goal_speeds(2, __pan_servo_id, pan_vel, __tilt_servo_id, tilt_vel); 00651 if (__move_pending) usleep(3000); 00652 } 00653 00654 if (__move_pending) { 00655 __value_mutex->lock(); 00656 __move_pending = false; 00657 float target_pan = __target_pan; 00658 float target_tilt = __target_tilt; 00659 __value_mutex->unlock(); 00660 exec_goto_pantilt(target_pan, target_tilt); 00661 } 00662 00663 try { 00664 __rx28->read_table_values(__pan_servo_id); 00665 __fresh_data = true; 00666 __rx28->read_table_values(__tilt_servo_id); 00667 } catch (Exception &e) { 00668 __logger->log_warn(name(), "Error while reading table values from servos, exception follows"); 00669 __logger->log_warn(name(), e); 00670 } 00671 00672 if (! is_final() || 00673 ! __rx28->is_torque_enabled(__pan_servo_id) || 00674 ! __rx28->is_torque_enabled(__tilt_servo_id)) { 00675 // while moving, and while the motor is off, wake us up to get new servo 00676 // position data 00677 wakeup(); 00678 } 00679 } 00680 00681 00682 /** Execute pan/tilt motion. 00683 * @param pan_rad pan in rad to move to 00684 * @param tilt_rad tilt in rad to move to 00685 */ 00686 void 00687 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad) 00688 { 00689 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) { 00690 __logger->log_warn(name(), "Pan value out of bounds, min: %f max: %f des: %f", 00691 __pan_min, __pan_max, pan_rad); 00692 return; 00693 } 00694 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) { 00695 __logger->log_warn(name(), "Tilt value out of bounds, min: %f max: %f des: %f", 00696 __tilt_min, __tilt_max, tilt_rad); 00697 return; 00698 } 00699 00700 unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0; 00701 00702 __rx28->get_angle_limits(__pan_servo_id, pan_min, pan_max); 00703 __rx28->get_angle_limits(__tilt_servo_id, tilt_min, tilt_max); 00704 00705 int pan_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * pan_rad) 00706 + RobotisRX28::CENTER_POSITION 00707 + __pan_zero_offset; 00708 int tilt_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * tilt_rad) 00709 + RobotisRX28::CENTER_POSITION 00710 + __tilt_zero_offset; 00711 00712 if ( (pan_pos < 0) || ((unsigned int)pan_pos < pan_min) || ((unsigned int)pan_pos > pan_max) ) { 00713 __logger->log_warn(name(), "Pan position out of bounds, min: %u max: %u des: %i", 00714 pan_min, pan_max, pan_pos); 00715 return; 00716 } 00717 00718 if ( (tilt_pos < 0) || ((unsigned int)tilt_pos < tilt_min) || ((unsigned int)tilt_pos > tilt_max) ) { 00719 __logger->log_warn(name(), "Tilt position out of bounds, min: %u max: %u des: %i", 00720 tilt_min, tilt_max, tilt_pos); 00721 return; 00722 } 00723 00724 __rx28->goto_positions(2, __pan_servo_id, pan_pos, __tilt_servo_id, tilt_pos); 00725 }