rx28_thread.cpp

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 }

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1