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 <aspect/blackboard.h>
26#include <aspect/blocked_timing.h>
27#include <aspect/configurable.h>
28#include <aspect/logging.h>
29#include <blackboard/interface_listener.h>
30#include <core/threading/read_write_lock.h>
31#include <core/threading/scoped_rwlock.h>
32#include <core/threading/thread.h>
33#include <utils/time/time.h>
34
35#ifdef USE_TIMETRACKER
36# include <utils/time/tracker.h>
37#endif
38#include <memory>
39#include <string>
40
41namespace fawkes {
42class DynamixelServoInterface;
43class LedInterface;
44class JointInterface;
45} // namespace fawkes
46
47class DynamixelChain;
48
49class Dynamixel;
50
56{
57public:
58 DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix);
59
60 virtual void init();
61 virtual void finalize();
62 virtual void loop();
63
64 // For BlackBoardInterfaceListener
66 fawkes::Message * message) noexcept;
67
68 void exec_sensor();
69 void exec_act();
70
71 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
72protected:
73 virtual void
75 {
76 Thread::run();
77 }
78
79private:
80 class Servo
81 {
82 public:
84 fawkes::LedInterface * led_if;
85 fawkes::JointInterface * joint_if;
86
87 fawkes::ReadWriteLock *value_rwlock;
88
89 bool move_pending;
90 float target_angle;
91 bool enable;
92 bool disable;
93 bool velo_pending;
94 unsigned int vel;
95 bool mode_set_pending;
96 bool recover_pending;
97 unsigned int new_mode;
98 bool led_enable;
99 bool led_disable;
100 float max_speed;
101 float angle_margin;
102 unsigned int torque_limit;
103 fawkes::Time time;
104 float last_angle;
105 };
106 std::map<unsigned int, Servo> servos_;
107
109 fawkes::ReadWriteLock * chain_rwlock_;
110
111 std::string cfg_prefix_;
112 std::string cfg_name_;
113 std::string cfg_device_;
114 unsigned int cfg_read_timeout_ms_;
115 unsigned int cfg_disc_timeout_ms_;
116 bool cfg_goto_zero_start_;
117 bool cfg_turn_off_;
118 unsigned int cfg_cw_compl_margin_;
119 unsigned int cfg_ccw_compl_margin_;
120 unsigned int cfg_cw_compl_slope_;
121 unsigned int cfg_ccw_compl_slope_;
122 float cfg_def_angle_margin_;
123 bool cfg_enable_echo_fix_;
124 bool cfg_enable_connection_stability_;
125 bool cfg_autorecover_enabled_;
126 unsigned char cfg_autorecover_flags_;
127 float cfg_torque_limit_;
128 unsigned char cfg_temperature_limit_;
129 bool cfg_prevent_alarm_shutdown_;
130 float cfg_prevent_alarm_shutdown_threshold_;
131 float cfg_min_voltage_;
132 float cfg_max_voltage_;
133 std::vector<unsigned int> cfg_servos_to_discover_;
134 bool cfg_enable_verbose_output_;
135
136 void goto_angle(unsigned int servo_id, float angle);
137 void goto_angle_timed(unsigned int servo_id, float angle, float time_sec);
138 float get_angle(unsigned int servo_id);
139 float get_angle(unsigned int servo_id, fawkes::Time &time);
140 void set_velocity(unsigned int servo_id, float vel);
141 float get_velocity(unsigned int servo_id);
142 void set_speed(unsigned int servo_id, unsigned int speed);
143 void set_mode(unsigned int servo_id, unsigned int new_mode);
144 void set_margin(unsigned int servo_id, float margin);
145 bool is_final(unsigned int servo_id);
146 bool is_enabled(unsigned int servo_id);
147 void set_enabled(unsigned int servo_id, bool enabled);
148 void set_led_enabled(unsigned int servo_id, bool enabled);
149 void stop_motion(unsigned int servo_id);
150 bool has_fresh_data();
151 void wait_for_fresh_data();
152
153 void exec_goto_angle(unsigned int servo_id, float angle);
154 void exec_set_mode(unsigned int servo_id, unsigned int new_mode);
155
156private:
157 fawkes::WaitCondition *update_waitcond_;
158
159 bool fresh_data_;
160 fawkes::Mutex *fresh_data_mutex_;
161};
162
163#endif
Class to access a chain of Robotis dynamixel servos.
Definition: servo_chain.h:37
Driver thread for Robotis dynamixel servos.
Definition: driver_thread.h:56
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: driver_thread.h:74
virtual void finalize()
Finalize the thread.
DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
void exec_sensor()
Update sensor values as necessary.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
void exec_act()
Process commands.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
BlackBoard interface listener.
Thread aspect to access configuration data.
Definition: configurable.h:33
DynamixelServoInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
JointInterface Fawkes BlackBoard Interface.
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:34
Thread aspect to log output.
Definition: logging.h:33
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Mutex mutual exclusion lock.
Definition: mutex.h:33
Read/write lock to allow multiple readers but only a single writer on the resource at a time.
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Wait until a given condition holds.
Fawkes library namespace.