00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00057 virtual bool bb_interface_message_received(fawkes::Interface *interface,
00058 fawkes::Message *message) throw();
00059
00060 void update_sensor_values();
00061
00062
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