Fawkes API  Fawkes Development Version
driver_thread.h
1 
2 /***************************************************************************
3  * driver_thread.h - Robotis dynamixel servo driver thread
4  *
5  * Created: Mon Mar 23 20:26:52 2015
6  * Copyright 2006-2015 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef __PLUGINS_DYNAMIXEL_DRIVER_THREAD_H_
23 #define __PLUGINS_DYNAMIXEL_DRIVER_THREAD_H_
24 
25 #include <core/threading/thread.h>
26 #include <aspect/blocked_timing.h>
27 #include <aspect/logging.h>
28 #include <aspect/configurable.h>
29 #include <aspect/blackboard.h>
30 
31 #include <core/threading/scoped_rwlock.h>
32 #include <core/threading/read_write_lock.h>
33 
34 #include <blackboard/interface_listener.h>
35 #include <utils/time/time.h>
36 
37 #ifdef USE_TIMETRACKER
38 # include <utils/time/tracker.h>
39 #endif
40 #include <string>
41 #include <memory>
42 
43 namespace fawkes {
44  class DynamixelServoInterface;
45  class LedInterface;
46  class JointInterface;
47 }
48 
49 class DynamixelChain;
50 
51 class Dynamixel;
52 
54 : public fawkes::Thread,
55  public fawkes::LoggingAspect,
59 {
60  public:
61  DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix);
62 
63  virtual void init();
64  virtual void finalize();
65  virtual void loop();
66 
67  // For BlackBoardInterfaceListener
68  virtual bool bb_interface_message_received(fawkes::Interface *interface,
69  fawkes::Message *message) throw();
70 
71  void exec_sensor();
72  void exec_act();
73 
74  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
75  protected: virtual void run() { Thread::run(); }
76 
77  private:
78 
79  class Servo {
80  public:
82  fawkes::LedInterface *led_if;
83  fawkes::JointInterface *joint_if;
84 
85  fawkes::ReadWriteLock *value_rwlock;
86 
87  bool move_pending;
88  float target_angle;
89  bool enable;
90  bool disable;
91  bool velo_pending;
92  unsigned int vel;
93  bool mode_set_pending;
94  bool recover_pending;
95  unsigned int new_mode;
96  bool led_enable;
97  bool led_disable;
98  float max_speed;
99  float angle_margin;
100  unsigned int torque_limit;
101  fawkes::Time time;
102  float last_angle;
103  };
104  std::map<unsigned int, Servo> servos_;
105 
107  fawkes::ReadWriteLock *chain_rwlock_;
108 
109  std::string cfg_prefix_;
110  std::string cfg_name_;
111  std::string cfg_device_;
112  unsigned int cfg_read_timeout_ms_;
113  unsigned int cfg_disc_timeout_ms_;
114  bool cfg_goto_zero_start_;
115  bool cfg_turn_off_;
116  unsigned int cfg_cw_compl_margin_;
117  unsigned int cfg_ccw_compl_margin_;
118  unsigned int cfg_cw_compl_slope_;
119  unsigned int cfg_ccw_compl_slope_;
120  float cfg_def_angle_margin_;
121  bool cfg_enable_echo_fix_;
122  bool cfg_enable_connection_stability_;
123  bool cfg_autorecover_enabled_;
124  unsigned char cfg_autorecover_flags_;
125  float cfg_torque_limit_;
126  unsigned char cfg_temperature_limit_;
127  bool cfg_prevent_alarm_shutdown_;
128  float cfg_prevent_alarm_shutdown_threshold_;
129  float cfg_min_voltage_;
130  float cfg_max_voltage_;
131  std::vector<unsigned int> cfg_servos_to_discover_;
132 
133  void goto_angle(unsigned int servo_id, float angle);
134  void goto_angle_timed(unsigned int servo_id, float angle, float time_sec);
135  float get_angle(unsigned int servo_id);
136  float get_angle(unsigned int servo_id, fawkes::Time &time);
137  void set_velocity(unsigned int servo_id, float vel);
138  float get_velocity(unsigned int servo_id);
139  void set_speed(unsigned int servo_id, unsigned int speed);
140  void set_mode(unsigned int servo_id, unsigned int new_mode);
141  void set_margin(unsigned int servo_id, float margin);
142  bool is_final(unsigned int servo_id);
143  bool is_enabled(unsigned int servo_id);
144  void set_enabled(unsigned int servo_id, bool enabled);
145  void set_led_enabled(unsigned int servo_id, bool enabled);
146  void stop_motion(unsigned int servo_id);
147  bool has_fresh_data();
148  void wait_for_fresh_data();
149 
150  void exec_goto_angle(unsigned int servo_id, float angle);
151  void exec_set_mode(unsigned int servo_id, unsigned int new_mode);
152 
153  private:
154  fawkes::WaitCondition *update_waitcond_;
155 
156  bool fresh_data_;
157  fawkes::Mutex *fresh_data_mutex_;
158 };
159 
160 #endif
Class to access a chain of Robotis dynamixel servos.
Definition: servo_chain.h:36
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Driver thread for Robotis dynamixel servos.
Definition: driver_thread.h:53
Fawkes library namespace.
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Read/write lock to allow multiple readers but only a single writer on the resource at a time...
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: driver_thread.h:75
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:33
DynamixelServoInterface Fawkes BlackBoard Interface.
JointInterface Fawkes BlackBoard Interface.
Mutex mutual exclusion lock.
Definition: mutex.h:32
BlackBoard interface listener.