rx28_thread.h

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