Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rx28_thread.h - RX28 pan/tilt unit act thread 00004 * 00005 * Created: Thu Jun 18 09:52:16 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 #ifndef __PLUGINS_PANTILT_ROBOTIS_RX28_THREAD_H_ 00024 #define __PLUGINS_PANTILT_ROBOTIS_RX28_THREAD_H_ 00025 00026 #include "../act_thread.h" 00027 00028 #include <blackboard/interface_listener.h> 00029 00030 #ifdef USE_TIMETRACKER 00031 # include <utils/time/tracker.h> 00032 #endif 00033 #include <string> 00034 #include <memory> 00035 00036 namespace fawkes { 00037 class PanTiltInterface; 00038 class LedInterface; 00039 } 00040 00041 class RobotisRX28; 00042 00043 class PanTiltRX28Thread 00044 : public PanTiltActThread, 00045 public fawkes::BlackBoardInterfaceListener 00046 { 00047 public: 00048 PanTiltRX28Thread(std::string &pantilt_cfg_prefix, 00049 std::string &ptu_cfg_prefix, 00050 std::string &ptu_name); 00051 00052 virtual void init(); 00053 virtual void finalize(); 00054 virtual void loop(); 00055 00056 // For BlackBoardInterfaceListener 00057 virtual bool bb_interface_message_received(fawkes::Interface *interface, 00058 fawkes::Message *message) throw(); 00059 00060 void update_sensor_values(); 00061 00062 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00063 protected: virtual void run() { Thread::run(); } 00064 00065 private: 00066 fawkes::PanTiltInterface *__pantilt_if; 00067 fawkes::LedInterface *__led_if; 00068 00069 fawkes::RefPtr<RobotisRX28> __rx28; 00070 00071 std::string __pantilt_cfg_prefix; 00072 std::string __ptu_cfg_prefix; 00073 std::string __ptu_name; 00074 std::string __cfg_device; 00075 unsigned int __cfg_read_timeout_ms; 00076 unsigned int __cfg_disc_timeout_ms; 00077 unsigned int __cfg_pan_servo_id; 00078 unsigned int __cfg_tilt_servo_id; 00079 int __cfg_pan_zero_offset; 00080 int __cfg_tilt_zero_offset; 00081 bool __cfg_goto_zero_start; 00082 bool __cfg_turn_off; 00083 unsigned int __cfg_cw_compl_margin; 00084 unsigned int __cfg_ccw_compl_margin; 00085 unsigned int __cfg_cw_compl_slope; 00086 unsigned int __cfg_ccw_compl_slope; 00087 float __cfg_pan_min; 00088 float __cfg_pan_max; 00089 float __cfg_tilt_min; 00090 float __cfg_tilt_max; 00091 float __cfg_pan_margin; 00092 float __cfg_tilt_margin; 00093 00094 class WorkerThread : public fawkes::Thread 00095 { 00096 public: 00097 WorkerThread(std::string ptu_name, fawkes::Logger *logger, 00098 fawkes::RefPtr<RobotisRX28> rx28, 00099 unsigned char pan_servo_id, unsigned char tilt_servo_id, 00100 float &pan_min, float &pan_max, float &tilt_min, float &tilt_max, 00101 int &pan_zero_offset, int &tilt_zero_offset); 00102 00103 ~WorkerThread(); 00104 void goto_pantilt(float pan, float tilt); 00105 void goto_pantilt_timed(float pan, float tilt, float time_sec); 00106 void get_pantilt(float &pan, float &tilt); 00107 void set_velocities(float pan_vel, float tilt_vel); 00108 void get_velocities(float &pan_vel, float &tilt_vel); 00109 void set_margins(float pan_margin, float tilt_margin); 00110 bool is_final(); 00111 bool is_enabled(); 00112 void set_enabled(bool enabled); 00113 void set_led_enabled(bool enabled); 00114 void stop_motion(); 00115 bool has_fresh_data(); 00116 00117 virtual void loop(); 00118 00119 private: 00120 void exec_goto_pantilt(float pan, float tilt); 00121 00122 private: 00123 fawkes::RefPtr<RobotisRX28> __rx28; 00124 fawkes::Logger *__logger; 00125 00126 unsigned char __pan_servo_id; 00127 unsigned char __tilt_servo_id; 00128 00129 float __pan_min; 00130 float __pan_max; 00131 float __tilt_min; 00132 float __tilt_max; 00133 int __pan_zero_offset; 00134 int __tilt_zero_offset; 00135 float __max_pan_speed; 00136 float __max_tilt_speed; 00137 float __pan_margin; 00138 float __tilt_margin; 00139 00140 fawkes::Mutex *__value_mutex; 00141 bool __move_pending; 00142 float __target_pan; 00143 float __target_tilt; 00144 bool __enable; 00145 bool __disable; 00146 bool __velo_pending; 00147 unsigned int __pan_vel; 00148 unsigned int __tilt_vel; 00149 bool __led_enable; 00150 bool __led_disable; 00151 00152 bool __fresh_data; 00153 }; 00154 00155 WorkerThread *__wt; 00156 }; 00157 00158 #endif