00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
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
00067
00068
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
00106 __rx28->set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ);
00107
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
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
00175 }
00176 }
00177
00178 __wt->cancel();
00179 __wt->join();
00180 delete __wt;
00181
00182
00183 __rx28 = NULL;
00184 }
00185
00186
00187
00188
00189
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
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;
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
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
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
00379 PanTiltRX28Thread::WorkerThread::~WorkerThread()
00380 {
00381 delete __value_mutex;
00382 }
00383
00384
00385
00386
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
00404
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
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
00432
00433
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
00447
00448
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
00495
00496
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
00527
00528
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
00542
00543
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
00551 }
00552
00553
00554
00555
00556
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
00570
00571
00572 bool
00573 PanTiltRX28Thread::WorkerThread::is_final()
00574 {
00575 MutexLocker lock(__value_mutex);
00576 float pan, tilt;
00577 get_pantilt(pan, tilt);
00578
00579
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
00588
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
00600
00601
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
00676
00677 wakeup();
00678 }
00679 }
00680
00681
00682
00683
00684
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 }