Fawkes API Fawkes Development Version
driver_thread.cpp
1
2/***************************************************************************
3 * driver_thread.cpp - Robotis dynamixel servo driver thread
4 *
5 * Created: Mon Mar 23 20:37:32 2015 (based on pantilt plugin)
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#include "driver_thread.h"
23
24#include "servo_chain.h"
25
26#include <core/threading/mutex_locker.h>
27#include <core/threading/read_write_lock.h>
28#include <core/threading/scoped_rwlock.h>
29#include <core/threading/wait_condition.h>
30#include <interfaces/DynamixelServoInterface.h>
31#include <interfaces/JointInterface.h>
32#include <interfaces/LedInterface.h>
33#include <utils/math/angle.h>
34#include <utils/misc/string_split.h>
35
36#include <algorithm>
37#include <cmath>
38#include <cstdarg>
39#include <cstring>
40#include <unistd.h>
41
42using namespace fawkes;
43
44/** @class DynamixelDriverThread "driver_thread.h"
45 * Driver thread for Robotis dynamixel servos.
46 * @author Tim Niemueller
47 */
48
49/** Constructor.
50 * @param cfg_prefix configuration prefix specific for the servo chain
51 * @param cfg_name name of the servo configuration
52 */
53DynamixelDriverThread::DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix)
54: Thread("DynamixelDriverThread", Thread::OPMODE_WAITFORWAKEUP),
55 BlackBoardInterfaceListener("DynamixelDriverThread(%s)", cfg_name.c_str())
56{
57 set_name("DynamixelDriverThread(%s)", cfg_name.c_str());
58
59 cfg_prefix_ = cfg_prefix;
60 cfg_name_ = cfg_name;
61}
62
63void
65{
66 cfg_device_ = config->get_string((cfg_prefix_ + "device").c_str());
67 cfg_read_timeout_ms_ = config->get_uint((cfg_prefix_ + "read_timeout_ms").c_str());
68 cfg_disc_timeout_ms_ = config->get_uint((cfg_prefix_ + "discover_timeout_ms").c_str());
69 cfg_goto_zero_start_ = config->get_bool((cfg_prefix_ + "goto_zero_start").c_str());
70 cfg_turn_off_ = config->get_bool((cfg_prefix_ + "turn_off").c_str());
71 cfg_cw_compl_margin_ = config->get_uint((cfg_prefix_ + "cw_compl_margin").c_str());
72 cfg_ccw_compl_margin_ = config->get_uint((cfg_prefix_ + "ccw_compl_margin").c_str());
73 cfg_cw_compl_slope_ = config->get_uint((cfg_prefix_ + "cw_compl_slope").c_str());
74 cfg_ccw_compl_slope_ = config->get_uint((cfg_prefix_ + "ccw_compl_slope").c_str());
75 cfg_def_angle_margin_ = config->get_float((cfg_prefix_ + "angle_margin").c_str());
76 cfg_enable_echo_fix_ = config->get_bool((cfg_prefix_ + "enable_echo_fix").c_str());
77 cfg_enable_connection_stability_ =
78 config->get_bool((cfg_prefix_ + "enable_connection_stability").c_str());
79 cfg_autorecover_enabled_ = config->get_bool((cfg_prefix_ + "autorecover_enabled").c_str());
80 cfg_autorecover_flags_ = config->get_uint((cfg_prefix_ + "autorecover_flags").c_str());
81 cfg_torque_limit_ = config->get_float((cfg_prefix_ + "torque_limit").c_str());
82 cfg_temperature_limit_ = config->get_uint((cfg_prefix_ + "temperature_limit").c_str());
83 cfg_prevent_alarm_shutdown_ = config->get_bool((cfg_prefix_ + "prevent_alarm_shutdown").c_str());
84 cfg_prevent_alarm_shutdown_threshold_ =
85 config->get_float((cfg_prefix_ + "prevent_alarm_shutdown_threshold").c_str());
86 cfg_min_voltage_ = config->get_float((cfg_prefix_ + "min_voltage").c_str());
87 cfg_max_voltage_ = config->get_float((cfg_prefix_ + "max_voltage").c_str());
88 cfg_servos_to_discover_ = config->get_uints((cfg_prefix_ + "servos").c_str());
89 cfg_enable_verbose_output_ = config->get_bool((cfg_prefix_ + "enable_verbose_output").c_str());
90
91 chain_ = new DynamixelChain(cfg_device_.c_str(),
92 cfg_read_timeout_ms_,
93 cfg_enable_echo_fix_,
94 cfg_enable_connection_stability_,
95 cfg_min_voltage_,
96 cfg_max_voltage_);
97 DynamixelChain::DeviceList devl = chain_->discover(cfg_disc_timeout_ms_, cfg_servos_to_discover_);
98 std::list<std::string> found_servos;
99 for (DynamixelChain::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
100 found_servos.push_back(std::to_string(*i));
101 Servo s;
102 s.servo_if = NULL;
103 s.led_if = NULL;
104 s.joint_if = NULL;
105
106 try {
107 s.servo_if = blackboard->open_for_writing_f<DynamixelServoInterface>("/dynamixel/%s/%u",
108 cfg_name_.c_str(),
109 *i);
110 s.led_if =
111 blackboard->open_for_writing_f<LedInterface>("/dynamixel/%s/%u", cfg_name_.c_str(), *i);
112 s.joint_if =
113 blackboard->open_for_writing_f<JointInterface>("/dynamixel/%s/%u", cfg_name_.c_str(), *i);
114
115 bbil_add_message_interface(s.servo_if);
116
117 } catch (Exception &e) {
118 blackboard->close(s.servo_if);
119 blackboard->close(s.led_if);
120 blackboard->close(s.joint_if);
121 throw;
122 }
123
124 s.move_pending = false;
125 s.mode_set_pending = false;
126 s.recover_pending = false;
127 s.target_angle = 0;
128 s.velo_pending = false;
129 s.vel = 0.;
130 s.enable = false;
131 s.disable = false;
132 s.led_enable = false;
133 s.led_disable = false;
134 s.last_angle = 0.f;
135 s.torque_limit = cfg_torque_limit_ * 0x3ff;
136 s.value_rwlock = new ReadWriteLock();
137 s.angle_margin = cfg_def_angle_margin_;
138
139 servos_[*i] = s;
140 }
141
142 logger->log_info(name(), "Found servos [%s]", str_join(found_servos, ",").c_str());
143
144 chain_rwlock_ = new ReadWriteLock();
145 fresh_data_mutex_ = new Mutex();
146 update_waitcond_ = new WaitCondition();
147
148 if (servos_.empty()) {
149 throw Exception("No servos found in chain %s", cfg_name_.c_str());
150 }
151
152 // We only want responses to be sent on explicit READ to speed up communication
154 // set compliance values
156 cfg_cw_compl_margin_,
157 cfg_cw_compl_slope_,
158 cfg_ccw_compl_margin_,
159 cfg_ccw_compl_slope_);
160
161 // set temperature limit
162 chain_->set_temperature_limit(DynamixelChain::BROADCAST_ID, cfg_temperature_limit_);
163
164 for (auto &sp : servos_) {
165 unsigned int servo_id = sp.first;
166 Servo & s = sp.second;
167
168 chain_->set_led_enabled(servo_id, false);
169 chain_->set_torque_enabled(servo_id, true);
170
171 chain_->read_table_values(servo_id);
172
173 s.max_speed = chain_->get_max_supported_speed(servo_id);
174
175 unsigned int cw_limit, ccw_limit;
176 chain_->get_angle_limits(servo_id, cw_limit, ccw_limit);
177
178 unsigned char cw_margin, cw_slope, ccw_margin, ccw_slope;
179 chain_->get_compliance_values(servo_id, cw_margin, cw_slope, ccw_margin, ccw_slope);
180
181 s.servo_if->set_model(chain_->get_model(servo_id));
182 s.servo_if->set_model_number(chain_->get_model_number(servo_id));
183 s.servo_if->set_cw_angle_limit(cw_limit);
184 s.servo_if->set_ccw_angle_limit(ccw_limit);
185 s.servo_if->set_temperature_limit(chain_->get_temperature_limit(servo_id));
186 s.servo_if->set_max_torque(chain_->get_max_torque(servo_id));
187 s.servo_if->set_mode(cw_limit == ccw_limit && cw_limit == 0 ? "WHEEL" : "JOINT");
188 s.servo_if->set_cw_slope(cw_slope);
189 s.servo_if->set_ccw_slope(ccw_slope);
190 s.servo_if->set_cw_margin(cw_margin);
191 s.servo_if->set_ccw_margin(ccw_margin);
192 s.servo_if->set_torque_limit(s.torque_limit);
193 s.servo_if->set_max_velocity(s.max_speed);
194 s.servo_if->set_enable_prevent_alarm_shutdown(cfg_prevent_alarm_shutdown_);
195 s.servo_if->set_autorecover_enabled(cfg_autorecover_enabled_);
196 s.servo_if->write();
197
198 s.servo_if->set_auto_timestamping(false);
199 }
200
201 if (cfg_goto_zero_start_) {
202 for (auto &s : servos_) {
203 goto_angle_timed(s.first, 0., 3.0);
204 }
205 }
206
208}
209
210void
212{
214
215 for (auto &s : servos_) {
216 blackboard->close(s.second.servo_if);
217 blackboard->close(s.second.led_if);
218 blackboard->close(s.second.joint_if);
219 }
220
221 delete chain_rwlock_;
222 delete fresh_data_mutex_;
223 delete update_waitcond_;
224
225 if (cfg_turn_off_) {
226 for (auto &s : servos_) {
227 try {
228 logger->log_debug(name(), "Turning off servo %s:%u", cfg_name_.c_str(), s.first);
229 chain_->set_led_enabled(s.first, false);
230 chain_->set_torque_enabled(s.first, false);
231 } catch (Exception &e) {
233 name(), "Failed to turn of servo %s:%u: %s", cfg_name_.c_str(), s.first, e.what());
234 }
235 }
236 // Give some time for shutdown comands to get through
237 usleep(10000);
238 }
239
240 // Setting to NULL deletes instance (RefPtr)
241 chain_ = NULL;
242}
243
244/** Update sensor values as necessary.
245 * To be called only from DynamixelSensorThread. Writes the current servo
246 * data into the interface.
247 */
248void
250{
251 if (has_fresh_data()) {
252 for (auto &sp : servos_) {
253 unsigned int servo_id = sp.first;
254 Servo & s = sp.second;
255
256 fawkes::Time time;
257 float angle = get_angle(servo_id, time);
258 float vel = get_velocity(servo_id);
259
260 // poor man's filter: only update if we get a change of least half a degree
261 if (fabs(s.last_angle - angle) >= deg2rad(0.5)) {
262 s.last_angle = angle;
263 } else {
264 angle = s.last_angle;
265 }
266
267 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
268
269 s.servo_if->set_timestamp(&s.time);
270 s.servo_if->set_position(chain_->get_position(servo_id));
271 s.servo_if->set_speed(chain_->get_speed(servo_id));
272 s.servo_if->set_goal_position(chain_->get_goal_position(servo_id));
273 s.servo_if->set_goal_speed(chain_->get_goal_speed(servo_id));
274 s.servo_if->set_load(chain_->get_load(servo_id));
275 s.servo_if->set_voltage(chain_->get_voltage(servo_id));
276 s.servo_if->set_temperature(chain_->get_temperature(servo_id));
277 s.servo_if->set_punch(chain_->get_punch(servo_id));
278 s.servo_if->set_angle(angle);
279 s.servo_if->set_velocity(vel);
280 s.servo_if->set_enabled(chain_->is_torque_enabled(servo_id));
281 s.servo_if->set_final(is_final(servo_id));
282 s.servo_if->set_velocity(get_velocity(servo_id));
283 s.servo_if->set_alarm_shutdown(chain_->get_alarm_shutdown(servo_id));
284
285 if (s.servo_if->is_enable_prevent_alarm_shutdown()) {
286 if ((chain_->get_load(servo_id) & 0x3ff)
287 > (cfg_prevent_alarm_shutdown_threshold_ * chain_->get_torque_limit(servo_id))) {
289 "Servo with ID: %d is in overload condition: torque_limit: %d, load: %d",
290 servo_id,
291 chain_->get_torque_limit(servo_id),
292 chain_->get_load(servo_id) & 0x3ff);
293 // is the current load cw or ccw?
294 if (chain_->get_load(servo_id) & 0x400) {
295 goto_angle(servo_id, get_angle(servo_id) + 0.001);
296 } else {
297 goto_angle(servo_id, get_angle(servo_id) - 0.001);
298 }
299 }
300 }
301
302 if (s.servo_if->is_autorecover_enabled()
303 && chain_->get_error(servo_id) & cfg_autorecover_flags_) {
304 logger->log_warn(name(), "Recovery for servo with ID: %d pending", servo_id);
305 s.recover_pending = true;
306 }
307
308 unsigned char cur_error = chain_->get_error(servo_id);
309 s.servo_if->set_error(s.servo_if->error() | cur_error);
310 if (cur_error) {
311 logger->log_error(name(), "Servo with ID: %d has error-flag: %d", servo_id, cur_error);
312 }
313 s.servo_if->write();
314
315 s.joint_if->set_position(angle);
316 s.joint_if->set_velocity(vel);
317 s.joint_if->write();
318 }
319 }
320}
321
322/** Process commands. */
323void
325{
326 for (auto &sp : servos_) {
327 unsigned int servo_id = sp.first;
328 Servo & s = sp.second;
329
330 s.servo_if->set_final(is_final(servo_id));
331
332 while (!s.servo_if->msgq_empty()) {
333 if (s.servo_if->msgq_first_is<DynamixelServoInterface::GotoMessage>()) {
334 DynamixelServoInterface::GotoMessage *msg = s.servo_if->msgq_first(msg);
335
336 goto_angle(servo_id, msg->angle());
337 s.servo_if->set_msgid(msg->id());
338 s.servo_if->set_final(false);
339
340 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::TimedGotoMessage>()) {
341 DynamixelServoInterface::TimedGotoMessage *msg = s.servo_if->msgq_first(msg);
342
343 goto_angle_timed(servo_id, msg->angle(), msg->time_sec());
344 s.servo_if->set_msgid(msg->id());
345 s.servo_if->set_final(false);
346
347 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetEnabledMessage>()) {
348 DynamixelServoInterface::SetEnabledMessage *msg = s.servo_if->msgq_first(msg);
349
350 set_enabled(servo_id, msg->is_enabled());
351
352 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetVelocityMessage>()) {
353 DynamixelServoInterface::SetVelocityMessage *msg = s.servo_if->msgq_first(msg);
354
355 if (msg->velocity() > s.servo_if->max_velocity()) {
357 "Desired velocity %f too high, max is %f",
358 msg->velocity(),
359 s.servo_if->max_velocity());
360 } else {
361 set_velocity(servo_id, msg->velocity());
362 }
363
364 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetMarginMessage>()) {
365 DynamixelServoInterface::SetMarginMessage *msg = s.servo_if->msgq_first(msg);
366
367 set_margin(servo_id, msg->angle_margin());
368 s.servo_if->set_angle_margin(msg->angle_margin());
369
370 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::ResetRawErrorMessage>()) {
371 s.servo_if->set_error(0);
372
373 } else if (s.servo_if
375 DynamixelServoInterface::SetPreventAlarmShutdownMessage *msg = s.servo_if->msgq_first(msg);
377
378 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetModeMessage>()) {
379 DynamixelServoInterface::SetModeMessage *msg = s.servo_if->msgq_first(msg);
380 set_mode(servo_id, msg->mode());
381
382 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetSpeedMessage>()) {
383 DynamixelServoInterface::SetSpeedMessage *msg = s.servo_if->msgq_first(msg);
384 set_speed(servo_id, msg->speed());
385
386 } else if (s.servo_if
388 DynamixelServoInterface::SetAutorecoverEnabledMessage *msg = s.servo_if->msgq_first(msg);
390
391 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetTorqueLimitMessage>()) {
392 DynamixelServoInterface::SetTorqueLimitMessage *msg = s.servo_if->msgq_first(msg);
393 s.recover_pending = true;
394 s.torque_limit = msg->torque_limit();
395
396 } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::RecoverMessage>()) {
397 s.recover_pending = true;
398
399 } else {
400 logger->log_warn(name(), "Unknown message received");
401 }
402
403 s.servo_if->msgq_pop();
404 }
405
406 s.servo_if->write();
407
408 bool write_led_if = false;
409 while (!s.led_if->msgq_empty()) {
410 write_led_if = true;
411 if (s.led_if->msgq_first_is<LedInterface::SetIntensityMessage>()) {
412 LedInterface::SetIntensityMessage *msg = s.led_if->msgq_first(msg);
413 set_led_enabled(servo_id, (msg->intensity() >= 0.5));
414 s.led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
415 } else if (s.led_if->msgq_first_is<LedInterface::TurnOnMessage>()) {
416 set_led_enabled(servo_id, true);
417 s.led_if->set_intensity(LedInterface::ON);
418 } else if (s.led_if->msgq_first_is<LedInterface::TurnOffMessage>()) {
419 set_led_enabled(servo_id, false);
420 s.led_if->set_intensity(LedInterface::OFF);
421 }
422
423 s.led_if->msgq_pop();
424 }
425 if (write_led_if)
426 s.led_if->write();
427 }
428}
429
430bool
432 Message * message) noexcept
433{
434 std::map<unsigned int, Servo>::iterator si =
435 std::find_if(servos_.begin(),
436 servos_.end(),
437 [interface](const std::pair<unsigned int, Servo> &sp) {
438 return (strcmp(sp.second.servo_if->uid(), interface->uid()) == 0);
439 });
440 if (si != servos_.end()) {
441 if (message->is_of_type<DynamixelServoInterface::StopMessage>()) {
442 stop_motion(si->first);
443 return false; // do not enqueue StopMessage
444 } else if (message->is_of_type<DynamixelServoInterface::FlushMessage>()) {
445 stop_motion(si->first);
446 if (cfg_enable_verbose_output_) {
447 logger->log_info(name(), "Flushing message queue");
448 }
449 si->second.servo_if->msgq_flush();
450 return false;
451 } else {
452 if (cfg_enable_verbose_output_) {
453 logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
454 }
455 return true;
456 }
457 }
458 return true;
459}
460
461/** Enable or disable servo.
462 * @param enabled true to enable servos, false to turn them off
463 */
464void
465DynamixelDriverThread::set_enabled(unsigned int servo_id, bool enabled)
466{
467 if (servos_.find(servo_id) == servos_.end()) {
469 "No servo with ID %u in chain %s, cannot set LED",
470 servo_id,
471 cfg_name_.c_str());
472 return;
473 }
474
475 Servo &s = servos_[servo_id];
476
477 logger->log_debug(name(), "Lock %d", __LINE__);
478 ScopedRWLock lock(s.value_rwlock);
479 if (enabled) {
480 s.enable = true;
481 s.disable = false;
482 } else {
483 s.enable = false;
484 s.disable = true;
485 }
486 wakeup();
487 logger->log_debug(name(), "UNLock %d", __LINE__);
488}
489
490/** Enable or disable LED.
491 * @param enabled true to enable LED, false to turn it off
492 */
493void
494DynamixelDriverThread::set_led_enabled(unsigned int servo_id, bool enabled)
495{
496 if (servos_.find(servo_id) == servos_.end()) {
498 "No servo with ID %u in chain %s, cannot set LED",
499 servo_id,
500 cfg_name_.c_str());
501 return;
502 }
503
504 Servo &s = servos_[servo_id];
505 logger->log_debug(name(), "Lock %d", __LINE__);
506 ScopedRWLock lock(s.value_rwlock);
507 if (enabled) {
508 s.led_enable = true;
509 s.led_disable = false;
510 } else {
511 s.led_enable = false;
512 s.led_disable = true;
513 }
514 wakeup();
515 logger->log_debug(name(), "UNLock %d", __LINE__);
516}
517
518/** Stop currently running motion. */
519void
520DynamixelDriverThread::stop_motion(unsigned int servo_id)
521{
522 if (servos_.find(servo_id) == servos_.end()) {
524 "No servo with ID %u in chain %s, cannot set LED",
525 servo_id,
526 cfg_name_.c_str());
527 return;
528 }
529
530 float angle = get_angle(servo_id);
531 goto_angle(servo_id, angle);
532}
533
534/** Goto desired angle value.
535 * @param angle in radians
536 */
537void
538DynamixelDriverThread::goto_angle(unsigned int servo_id, float angle)
539{
540 if (servos_.find(servo_id) == servos_.end()) {
542 "No servo with ID %u in chain %s, cannot set LED",
543 servo_id,
544 cfg_name_.c_str());
545 return;
546 }
547
548 Servo &s = servos_[servo_id];
549
550 logger->log_debug(name(), "Lock %d", __LINE__);
551 ScopedRWLock lock(s.value_rwlock);
552 s.target_angle = angle;
553 s.move_pending = true;
554 wakeup();
555 logger->log_debug(name(), "UNLock %d", __LINE__);
556}
557
558/** Goto desired angle value in a specified time.
559 * @param angle in radians
560 * @param time_sec time when to reach the desired angle value
561 */
562void
563DynamixelDriverThread::goto_angle_timed(unsigned int servo_id, float angle, float time_sec)
564{
565 if (servos_.find(servo_id) == servos_.end()) {
567 "No servo with ID %u in chain %s, cannot set LED",
568 servo_id,
569 cfg_name_.c_str());
570 return;
571 }
572 Servo &s = servos_[servo_id];
573
574 s.target_angle = angle;
575 s.move_pending = true;
576
577 float cangle = get_angle(servo_id);
578 float angle_diff = fabs(angle - cangle);
579 float req_angle_vel = angle_diff / time_sec;
580
581 if (req_angle_vel > s.max_speed) {
583 "Requested move to %f in %f sec requires a "
584 "angle speed of %f rad/s, which is greater than the maximum "
585 "of %f rad/s, reducing to max",
586 angle,
587 time_sec,
588 req_angle_vel,
589 s.max_speed);
590 req_angle_vel = s.max_speed;
591 }
592 set_velocity(servo_id, req_angle_vel);
593
594 wakeup();
595}
596
597/** Set desired velocity.
598 * @param vel the desired velocity in rad/s
599 */
600void
601DynamixelDriverThread::set_velocity(unsigned int servo_id, float vel)
602{
603 if (servos_.find(servo_id) == servos_.end()) {
605 "No servo with ID %u in chain %s, cannot set velocity",
606 servo_id,
607 cfg_name_.c_str());
608 return;
609 }
610 Servo &s = servos_[servo_id];
611
612 float velo_tmp = roundf((vel / s.max_speed) * DynamixelChain::MAX_SPEED);
613 set_speed(servo_id, (unsigned int)velo_tmp);
614}
615
616/** Set desired speed.
617 * When the servo is set to wheel mode, bit 10 of the speed value is used
618 * to either move cw (1) or ccw (0).
619 * @param speed the speed
620 */
621void
622DynamixelDriverThread::set_speed(unsigned int servo_id, unsigned int speed)
623{
624 if (servos_.find(servo_id) == servos_.end()) {
626 "No servo with ID %u in chain %s, cannot set speed",
627 servo_id,
628 cfg_name_.c_str());
629 return;
630 }
631 Servo &s = servos_[servo_id];
632
633 ScopedRWLock lock(s.value_rwlock);
634 if (speed <= DynamixelChain::MAX_SPEED) {
635 s.vel = speed;
636 s.velo_pending = true;
637 } else {
639 "Calculated velocity value out of bounds, "
640 "min: 0 max: %u des: %u",
642 speed);
643 }
644}
645
646/** Set desired mode.
647 * @param mode, either DynamixelServoInterface.JOINT or DynamixelServoInterface.WHEEL
648 */
649void
650DynamixelDriverThread::set_mode(unsigned int servo_id, unsigned int mode)
651{
652 if (servos_.find(servo_id) == servos_.end()) {
654 "No servo with ID %u in chain %s, cannot set mode",
655 servo_id,
656 cfg_name_.c_str());
657 return;
658 }
659 Servo &s = servos_[servo_id];
660
661 ScopedRWLock(s.value_rwlock);
662 s.mode_set_pending = true;
663 s.new_mode = mode;
664 s.servo_if->set_mode(mode == DynamixelServoInterface::JOINT ? "JOINT" : "WHEEL");
665}
666
667/** Get current velocity.
668 */
669float
670DynamixelDriverThread::get_velocity(unsigned int servo_id)
671{
672 if (servos_.find(servo_id) == servos_.end()) {
674 "No servo with ID %u in chain %s, cannot set velocity",
675 servo_id,
676 cfg_name_.c_str());
677 return 0.;
678 }
679 Servo &s = servos_[servo_id];
680
681 unsigned int velticks = chain_->get_speed(servo_id);
682
683 return (((float)velticks / (float)DynamixelChain::MAX_SPEED) * s.max_speed);
684}
685
686/** Set desired angle margin.
687 * @param angle_margin the desired angle_margin
688 */
689void
690DynamixelDriverThread::set_margin(unsigned int servo_id, float angle_margin)
691{
692 if (servos_.find(servo_id) == servos_.end()) {
694 "No servo with ID %u in chain %s, cannot set velocity",
695 servo_id,
696 cfg_name_.c_str());
697 return;
698 }
699 Servo &s = servos_[servo_id];
700 if (angle_margin > 0.0)
701 s.angle_margin = angle_margin;
702}
703
704/** Get angle - the position from -2.62 to + 2.62 (-150 to +150 degrees)
705 */
706float
707DynamixelDriverThread::get_angle(unsigned int servo_id)
708{
709 if (servos_.find(servo_id) == servos_.end()) {
711 "No servo with ID %u in chain %s, cannot set velocity",
712 servo_id,
713 cfg_name_.c_str());
714 return 0.;
715 }
716
717 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
718
719 int ticks = ((int)chain_->get_position(servo_id) - (int)DynamixelChain::CENTER_POSITION);
720
722}
723
724/** Get angle value with time.
725 * @param time upon return contains the time the angle value was read
726 */
727float
728DynamixelDriverThread::get_angle(unsigned int servo_id, fawkes::Time &time)
729{
730 if (servos_.find(servo_id) == servos_.end()) {
732 "No servo with ID %u in chain %s, cannot set velocity",
733 servo_id,
734 cfg_name_.c_str());
735 return 0.;
736 }
737 Servo &s = servos_[servo_id];
738
739 time = s.time;
740 return get_angle(servo_id);
741}
742
743/** Check if motion is final.
744 * @return true if motion is final, false otherwise
745 */
746bool
747DynamixelDriverThread::is_final(unsigned int servo_id)
748{
749 if (servos_.find(servo_id) == servos_.end()) {
751 "No servo with ID %u in chain %s, cannot set velocity",
752 servo_id,
753 cfg_name_.c_str());
754 return 0.;
755 }
756 Servo &s = servos_[servo_id];
757
758 float angle = get_angle(servo_id);
759
760 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
761
762 return ((fabs(angle - s.target_angle) <= s.angle_margin) || (!chain_->is_moving(servo_id)));
763}
764
765/** Check if servo is enabled.
766 * @return true if torque is enabled for both servos, false otherwise
767 */
768bool
769DynamixelDriverThread::is_enabled(unsigned int servo_id)
770{
771 return chain_->is_torque_enabled(servo_id);
772}
773
774/** Check is fresh sensor data is available.
775 * Note that this method will return true at once per sensor update cycle.
776 * @return true if fresh data is available, false otherwise
777 */
778bool
779DynamixelDriverThread::has_fresh_data()
780{
781 MutexLocker lock(fresh_data_mutex_);
782
783 bool rv = fresh_data_;
784 fresh_data_ = false;
785 return rv;
786}
787
788void
790{
791 for (auto &sp : servos_) {
792 unsigned int servo_id = sp.first;
793 Servo & s = sp.second;
794 if (s.enable) {
795 s.value_rwlock->lock_for_write();
796 s.enable = false;
797 s.value_rwlock->unlock();
798 ScopedRWLock lock(chain_rwlock_);
799 chain_->set_led_enabled(servo_id, true);
800 chain_->set_torque_enabled(servo_id, true);
801 if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
802 || s.recover_pending)
803 usleep(3000);
804 } else if (s.disable) {
805 s.value_rwlock->lock_for_write();
806 s.disable = false;
807 s.value_rwlock->unlock();
808 ScopedRWLock lock(chain_rwlock_);
809 chain_->set_torque_enabled(servo_id, false);
810 if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
811 || s.recover_pending)
812 usleep(3000);
813 }
814
815 if (s.led_enable) {
816 s.value_rwlock->lock_for_write();
817 s.led_enable = false;
818 s.value_rwlock->unlock();
819 ScopedRWLock lock(chain_rwlock_);
820 chain_->set_led_enabled(servo_id, true);
821 if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
822 usleep(3000);
823 } else if (s.led_disable) {
824 s.value_rwlock->lock_for_write();
825 s.led_disable = false;
826 s.value_rwlock->unlock();
827 ScopedRWLock lock(chain_rwlock_);
828 chain_->set_led_enabled(servo_id, false);
829 if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
830 usleep(3000);
831 }
832
833 if (s.velo_pending) {
834 s.value_rwlock->lock_for_write();
835 s.velo_pending = false;
836 unsigned int vel = s.vel;
837 s.value_rwlock->unlock();
838 ScopedRWLock lock(chain_rwlock_);
839 chain_->set_goal_speed(servo_id, vel);
840 if (s.move_pending || s.mode_set_pending || s.recover_pending)
841 usleep(3000);
842 }
843
844 if (s.move_pending) {
845 s.value_rwlock->lock_for_write();
846 s.move_pending = false;
847 float target_angle = s.target_angle;
848 s.value_rwlock->unlock();
849 exec_goto_angle(servo_id, target_angle);
850 if (s.mode_set_pending || s.recover_pending)
851 usleep(3000);
852 }
853
854 if (s.mode_set_pending) {
855 s.value_rwlock->lock_for_write();
856 s.mode_set_pending = false;
857 exec_set_mode(servo_id, s.new_mode);
858 s.value_rwlock->unlock();
859 if (s.recover_pending)
860 usleep(3000);
861 }
862
863 if (s.recover_pending) {
864 s.value_rwlock->lock_for_write();
865 s.recover_pending = false;
866 chain_->set_torque_limit(servo_id, s.torque_limit);
867 s.value_rwlock->unlock();
868 }
869
870 try {
871 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
872 chain_->read_table_values(servo_id);
873
874 MutexLocker lock_fresh_data(fresh_data_mutex_);
875 fresh_data_ = true;
876 s.time.stamp();
877 } catch (Exception &e) {
878 // usually just a timeout, too noisy
879 //logger_->log_warn(name(), "Error while reading table values from servos, exception follows");
880 //logger_->log_warn(name(), e);
881 }
882 }
883
884 update_waitcond_->wake_all();
885
886 // Wakeup ourselves for faster updates
887 wakeup();
888}
889
890/** Execute angle motion.
891 * @param angle_rad angle in rad to move to
892 */
893void
894DynamixelDriverThread::exec_goto_angle(unsigned int servo_id, float angle_rad)
895{
896 unsigned int pos_min = 0, pos_max = 0;
897 chain_->get_angle_limits(servo_id, pos_min, pos_max);
898
899 int pos =
901
902 if ((pos < 0) || ((unsigned int)pos < pos_min) || ((unsigned int)pos > pos_max)) {
904 name(), "Position out of bounds, min: %u max: %u des: %i", pos_min, pos_max, pos);
905 return;
906 }
907
908 ScopedRWLock lock(chain_rwlock_);
909 chain_->goto_position(servo_id, pos);
910}
911
912/** Execute set mode.
913 * @param new_mode - either DynamixelServoInterface::JOINT or DynamixelServoInterface::WHEEL
914 */
915void
916DynamixelDriverThread::exec_set_mode(unsigned int servo_id, unsigned int new_mode)
917{
918 if (new_mode == DynamixelServoInterface::JOINT) {
919 ScopedRWLock lock(chain_rwlock_);
920 chain_->set_angle_limits(servo_id, 0, 1023);
921 } else if (new_mode == DynamixelServoInterface::WHEEL) {
922 ScopedRWLock lock(chain_rwlock_);
923 chain_->set_angle_limits(servo_id, 0, 0);
924 } else {
925 logger->log_error(name(), "Mode %d cannot be set - unknown", new_mode);
926 }
927
928 return;
929}
930
931/** Wait for fresh data to be received.
932 * Blocks the calling thread.
933 */
934void
935DynamixelDriverThread::wait_for_fresh_data()
936{
937 update_waitcond_->wait();
938}
Class to access a chain of Robotis dynamixel servos.
Definition: servo_chain.h:37
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: servo_chain.h:40
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: servo_chain.h:145
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
void set_compliance_values(unsigned char id, unsigned char cw_margin, unsigned char cw_slope, unsigned char ccw_margin, unsigned char ccw_slope)
Set compliance values.
unsigned int get_model_number(unsigned char id, bool refresh=false)
Get model.
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > &servos=std::vector< unsigned int >())
Discover devices on the bus.
void get_compliance_values(unsigned char id, unsigned char &cw_margin, unsigned char &cw_slope, unsigned char &ccw_margin, unsigned char &ccw_slope, bool refresh=false)
Get compliance values.
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
void read_table_values(unsigned char id)
Read all table values for given servo.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
Definition: servo_chain.h:150
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
static const unsigned int MAX_SPEED
MAX_SPEED.
Definition: servo_chain.h:157
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
Definition: servo_chain.h:154
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: servo_chain.h:153
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
unsigned char get_error(unsigned char id)
Get error flags set by the servo.
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: servo_chain.h:148
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
const char * get_model(unsigned char id, bool refresh=false)
Get model string.
unsigned char get_alarm_shutdown(unsigned char id, bool refresh=false)
Get shutdown on alarm state.
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
unsigned int get_load(unsigned char id, bool refresh=false)
Get current load.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing_f(const char *interface_type, const char *identifier,...)
Open interface for writing with identifier format string.
Definition: blackboard.cpp:319
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual std::vector< unsigned int > get_uints(const char *path)=0
Get list of values from configuration which is of type unsigned int.
FlushMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
RecoverMessage Fawkes BlackBoard Interface Message.
ResetRawErrorMessage Fawkes BlackBoard Interface Message.
SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
void set_autorecover_enabled(const bool new_autorecover_enabled)
Set autorecover_enabled value.
SetEnabledMessage Fawkes BlackBoard Interface Message.
SetMarginMessage Fawkes BlackBoard Interface Message.
SetModeMessage Fawkes BlackBoard Interface Message.
SetPreventAlarmShutdownMessage Fawkes BlackBoard Interface Message.
bool is_enable_prevent_alarm_shutdown() const
Get enable_prevent_alarm_shutdown value.
void set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
Set enable_prevent_alarm_shutdown value.
SetSpeedMessage Fawkes BlackBoard Interface Message.
SetTorqueLimitMessage Fawkes BlackBoard Interface Message.
SetVelocityMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
TimedGotoMessage Fawkes BlackBoard Interface Message.
DynamixelServoInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
JointInterface Fawkes BlackBoard Interface.
SetIntensityMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:56
float intensity() const
Get intensity value.
TurnOffMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:107
TurnOnMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:87
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:34
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Mutex locking helper.
Definition: mutex_locker.h:34
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.
Scoped read/write lock.
Definition: scoped_rwlock.h:34
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
void wakeup()
Wake up thread.
Definition: thread.cpp:995
A class for handling time.
Definition: time.h:93
Wait until a given condition holds.
void wait()
Wait for the condition forever.
void wake_all()
Wake up all waiting threads.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
std::string str_join(const InputIterator &first, const InputIterator &last, char delim='/')
Join list of strings string using given delimiter.
Definition: string_split.h:139