DYNAMIXEL Driver 0.1.0
API reference
Loading...
Searching...
No Matches
dynamixel.hpp
Go to the documentation of this file.
1// clang-format off: the tables below must keep one row per line.
18// clang-format on
19#ifndef DYNAMIXEL_HPP
20#define DYNAMIXEL_HPP
21
22#include <algorithm>
23#include <array>
24#include <cstddef>
25#include <cstdint>
26#include <span>
27
28#include "dynamixel_detail.hpp"
30#include "dynamixel_tables.hpp"
31#include "dynamixel_types.hpp"
33#include "dynamixel_version.h"
34
35namespace dynamixel {
36
37namespace detail {
38
39// Optional hooks: a `Declares*` concept only tests the name, the `Has*` concept the exact
40// signature, `noexcept` and the result type; the driver rejects a hook that is declared but does
41// not match (KB-CPP20-007).
42
44template <typename Port>
45concept DeclaresSetBaudRate = requires(Port& port) { port.SetBaudRate(1_u32); };
46
48template <typename Port>
49concept HasSetBaudRate =
50 requires { ExactHook<Port, bool(uint32_t) noexcept>::Take(&Port::SetBaudRate); };
51
53template <typename Port>
54concept DeclaresLog = requires(Port& port) { port.Log(LogLevel::Info, ""); };
55
57template <typename Port>
58concept HasLog =
59 requires { ExactHook<Port, void(LogLevel, const char*) noexcept>::Take(&Port::Log); };
60
62template <typename Port>
63void Log(Port& port, [[maybe_unused]] LogLevel level,
64 [[maybe_unused]] const char* message) noexcept {
65 if constexpr (HasLog<Port>) {
66 port.Log(level, message);
67 } else {
68 static_cast<void>(port);
69 }
70}
71
74struct MultiRequest {
75 bool fast;
76 bool bulk;
77 uint16_t address;
78 uint16_t length;
79 std::span<const uint8_t> ids;
80 std::span<const BulkReadItem> items;
81
83 [[nodiscard]] std::size_t Count() const noexcept {
84 return bulk ? items.size() : ids.size();
85 }
87 [[nodiscard]] uint8_t IdAt(std::size_t k) const noexcept {
88 return bulk ? items[k].id : ids[k];
89 }
91 [[nodiscard]] std::size_t LengthAt(std::size_t k) const noexcept {
92 return bulk ? static_cast<std::size_t>(items[k].length) : static_cast<std::size_t>(length);
93 }
95 [[nodiscard]] std::size_t AddressAt(std::size_t k) const noexcept {
96 return bulk ? static_cast<std::size_t>(items[k].address) : static_cast<std::size_t>(address);
97 }
99 [[nodiscard]] bool ListedAfter(std::size_t k, uint8_t id) const noexcept {
100 bool later = false;
101 for (std::size_t j = k + 1U; j < Count(); ++j) {
102 later = later || (IdAt(j) == id);
103 }
104 return later;
105 }
106};
107
109struct FastCursor {
110 std::size_t pos;
111 std::size_t have;
112 bool open;
113 Error stop;
114};
115
118struct StopTally {
119 Error last{Error::Bus};
120 bool sent{false};
121 bool confirmed{false};
122 bool flush_fault{false};
123};
124
125} // namespace detail
126
133// clang-format off
157// clang-format on
158template <UartPort Port, std::size_t FrameBytes = 512>
160 static_assert((FrameBytes >= 64U) && (FrameBytes <= 4096U),
161 "dynamixel: FrameBytes must be 64 to 4096");
162 static_assert(
163 !detail::DeclaresSetBaudRate<Port> || detail::HasSetBaudRate<Port>,
164 "dynamixel: the optional SetBaudRate hook must be `bool SetBaudRate(uint32_t) noexcept`");
165 static_assert(!detail::DeclaresLog<Port> || detail::HasLog<Port>,
166 "dynamixel: the optional Log hook must be `void Log(LogLevel, const char* message) "
167 "noexcept`");
168
169public:
175 explicit DYNAMIXEL(Port& port, const Config& config = {}) noexcept
176 : port_(port), config_(config) {}
177
179 DYNAMIXEL(const DYNAMIXEL&) = delete;
181 DYNAMIXEL& operator=(const DYNAMIXEL&) = delete;
182
185
195
197 [[nodiscard]] bool IsInitialized() const noexcept {
198 return initialized_;
199 }
200
207
214 Result<void> Resync() noexcept;
215
222 Result<void> ApplyConfig(const Config& config) noexcept;
223
225 [[nodiscard]] const Config& GetConfig() const& noexcept {
226 return config_;
227 }
229 void GetConfig() const&& = delete;
230
234
243 Result<PingInfo> Ping(uint8_t id) noexcept;
244
252 Result<PingInfo> Identify(uint8_t id) noexcept;
253
267 Result<uint32_t> BroadcastPing(std::span<PingInfo> found) noexcept;
268
277 Result<uint32_t> ScanIds(uint8_t first, uint8_t last, std::span<PingInfo> found) noexcept;
278
282
294 Result<void> Read(uint8_t id, uint16_t address, std::span<uint8_t> out) noexcept;
295
307 Result<void> Write(uint8_t id, uint16_t address, std::span<const uint8_t> data) noexcept;
308
316 Result<void> RegWrite(uint8_t id, uint16_t address, std::span<const uint8_t> data) noexcept;
317
323 Result<void> Action(uint8_t id) noexcept;
324
328
342 Result<void> FactoryReset(uint8_t id, ResetOption option) noexcept;
343
351 Result<void> Reboot(uint8_t id) noexcept;
352
361 Result<void> Clear(uint8_t id, ClearOption option) noexcept;
362
377 Result<void> Backup(uint8_t id, BackupOption option) noexcept;
378
382
393 Result<void> SyncWrite(uint16_t address, uint16_t length, std::span<const uint8_t> ids,
394 std::span<const uint8_t> data) noexcept;
395
402 Result<void> BulkWrite(std::span<const BulkWriteItem> items) noexcept;
403
417 Result<void> SyncRead(uint16_t address, uint16_t length, std::span<const uint8_t> ids,
418 std::span<uint8_t> out, std::span<DeviceReply> replies) noexcept;
419
432 Result<void> FastSyncRead(uint16_t address, uint16_t length, std::span<const uint8_t> ids,
433 std::span<uint8_t> out, std::span<DeviceReply> replies) noexcept;
434
443 Result<void> BulkRead(std::span<const BulkReadItem> items, std::span<uint8_t> out,
444 std::span<DeviceReply> replies) noexcept;
445
453 Result<void> FastBulkRead(std::span<const BulkReadItem> items, std::span<uint8_t> out,
454 std::span<DeviceReply> replies) noexcept;
455
459
467 Result<int32_t> ReadRegister(ServoRef servo, Reg reg) noexcept;
468
478 Result<void> WriteRegister(ServoRef servo, Reg reg, int32_t value) noexcept;
479
486 Result<void> SetTorque(ServoRef servo, bool enable) noexcept;
487
500 Result<void> DisableTorque(ServoRef servo) noexcept;
501
509 Result<void> DisableTorqueAll(Family family) noexcept;
510
527 Result<void> DisableTorqueAt(uint8_t id, uint16_t torque_address) noexcept;
528
535 Result<void> DisableTorqueEach(std::span<const ServoRef> servos) noexcept;
536
543
550 Result<void> SetGoalPosition(ServoRef servo, int32_t counts) noexcept;
551
559 Result<void> SetId(ServoRef servo, uint8_t new_id) noexcept;
560
568 Result<void> SetServoBaudRate(ServoRef servo, uint32_t bits_per_second) noexcept;
569
577 Result<void> ChangeBusBaud(uint32_t bits_per_second) noexcept;
578
588 Result<void> SetStatusReturnLevel(ServoRef servo, StatusReturn level) noexcept;
589
593
595 [[nodiscard]] const ReplyInfo& GetLastReply() const& noexcept {
596 return last_reply_;
597 }
599 void GetLastReply() const&& = delete;
600
603 [[nodiscard]] const ReplyInfo& GetLatchedAlert() const& noexcept {
604 return latched_alert_;
605 }
607 void GetLatchedAlert() const&& = delete;
608
610 void ClearLatchedAlert() noexcept {
611 latched_alert_ = ReplyInfo{};
612 }
613
615 [[nodiscard]] const Stats& GetStats() const& noexcept {
616 return stats_;
617 }
619 void GetStats() const&& = delete;
620
622 void ResetStats() noexcept {
623 stats_ = Stats{};
624 }
625
630 [[nodiscard]] std::span<const uint8_t> GetLastRequest() const& noexcept {
631 return std::span<const uint8_t>{tx_.data(), tx_len_};
632 }
634 void GetLastRequest() const&& = delete;
635
637
638private:
640 enum class ReplyRule : uint8_t {
641 ReadLevel,
642 WriteLevel,
643 };
644
645 [[nodiscard]] bool validId(uint8_t id, bool allow_broadcast) const noexcept;
646 [[nodiscard]] bool replyExpected(ReplyRule rule, uint8_t id) const noexcept;
647 [[nodiscard]] static bool retryable(Error error) noexcept;
648 [[nodiscard]] static std::size_t stuffedBound(std::size_t bytes) noexcept;
649 [[nodiscard]] static uint32_t saneTimeout(uint32_t us) noexcept;
650 Error ready() const noexcept;
651 Error sendFrame(std::size_t len, bool stop) noexcept;
652 Error readEcho(std::size_t len) noexcept;
653 void dropFront(std::size_t count) noexcept;
654 Error receiveFrame(uint32_t first_timeout_us, std::size_t& frame_size) noexcept;
655 Error acceptStatus(std::size_t frame_size, uint8_t expect_id, std::size_t expect_params,
656 std::span<uint8_t>& data) noexcept;
657 Error transactOnce(std::size_t len, uint8_t id, bool expect_reply, std::size_t expect_params,
658 bool stop, std::span<uint8_t>& data) noexcept;
659 [[nodiscard]] const RegInfo* lookup(Family family, Reg reg) const noexcept;
660 bool startForStop() noexcept;
661 Error stopOnce(uint16_t address, uint8_t id, bool answer) noexcept;
662 bool noteStop(detail::StopTally& tally, Error attempt, bool answer) const noexcept;
663 Result<void> stopOutcome(const detail::StopTally& tally, bool answer) noexcept;
664 Error pingReply(std::size_t frame_size, std::span<const PingInfo> known, PingInfo& info) noexcept;
665 Error transact(std::size_t len, uint8_t id, bool expect_reply, std::size_t expect_params,
666 bool idempotent, std::span<uint8_t>& data) noexcept;
667 Result<void> sendWrite(protocol::Instruction instruction, uint8_t id, uint16_t address,
668 std::span<const uint8_t> data, bool settle) noexcept;
669 Result<void> sendSimple(protocol::Instruction instruction, uint8_t id,
670 std::span<const uint8_t> params, bool settle) noexcept;
671 Result<void> multiRead(const detail::MultiRequest& rq, std::span<uint8_t> out,
672 std::span<DeviceReply> replies) noexcept;
673 [[nodiscard]] bool multiItemBad(const detail::MultiRequest& rq, std::size_t k) const noexcept;
674 [[nodiscard]] bool multiBad(const detail::MultiRequest& rq, std::span<uint8_t> out,
675 std::span<DeviceReply> replies) const noexcept;
676 [[nodiscard]] static protocol::Instruction multiInstruction(bool fast, bool bulk) noexcept;
677 std::size_t buildMultiRead(const detail::MultiRequest& rq) noexcept;
678 Error separateReceive(const detail::MultiRequest& rq, std::size_t k,
679 std::span<uint8_t>& data) noexcept;
680 Error collectSeparate(const detail::MultiRequest& rq, std::span<uint8_t> out,
681 std::span<DeviceReply> replies) noexcept;
682 [[nodiscard]] bool fastPartCrcOk(std::size_t pos, std::size_t part) const noexcept;
683 void fastAccept(uint8_t id, std::size_t want, std::size_t pos, std::span<uint8_t> out,
684 std::size_t offset, DeviceReply& reply) noexcept;
685 void fastPart(const detail::MultiRequest& rq, std::size_t k, detail::FastCursor& cur,
686 std::span<uint8_t> out, std::size_t offset, DeviceReply& reply) noexcept;
687 Error collectFast(const detail::MultiRequest& rq, std::span<uint8_t> out,
688 std::span<DeviceReply> replies) noexcept;
689 Result<void> applyRate() noexcept;
690 Result<void> fail(Error error, const char* message) noexcept;
691 void noteReply(const ReplyInfo& info) noexcept;
692
694 static constexpr std::size_t kScratchBytes = 64;
696 static constexpr uint32_t kStopAttempts = 3;
697
698 Port& port_;
699 Config config_{};
700 std::array<uint8_t, FrameBytes> tx_{};
701 std::array<uint8_t, FrameBytes> rx_{};
702 std::array<uint8_t, kScratchBytes> scratch_{};
703 std::size_t tx_len_{0};
704 bool flush_fault_{false};
705 std::size_t rx_have_{0};
706 ReplyInfo last_reply_{};
707 ReplyInfo latched_alert_{};
708 uint16_t alt_id_{
709 0x100U};
710 std::size_t rx_drop_{
711 0};
712 Stats stats_{};
713 bool initialized_{false};
714};
715
718} // namespace dynamixel
719
720// codeql::misra_deviation_next_line(MD-CPP-009)
721#include "../src/dynamixel.ipp" // IWYU pragma: export
722
723#endif // DYNAMIXEL_HPP
Master of one DYNAMIXEL bus on a platform port.
Definition dynamixel.hpp:159
Result< PingInfo > Identify(uint8_t id) noexcept
Ping and learn the model: protocol 2.0 is one ping, protocol 1.0 a ping and a read of the Model Numbe...
Result< void > SyncWrite(uint16_t address, uint16_t length, std::span< const uint8_t > ids, std::span< const uint8_t > data) noexcept
Write the same address and length to several servos with one packet (both protocols)....
void ResetStats() noexcept
Zero the counters. No I/O.
Definition dynamixel.hpp:622
Result< void > SyncRead(uint16_t address, uint16_t length, std::span< const uint8_t > ids, std::span< uint8_t > out, std::span< DeviceReply > replies) noexcept
Read the same address and length from several servos (protocol 2.0): each one answers with its own pa...
Result< void > RegWrite(uint8_t id, uint16_t address, std::span< const uint8_t > data) noexcept
Store a write to be executed by the next Action instruction (Reg Write).
void GetLatchedAlert() const &&=delete
Not on a temporary driver (KB-CPP20-044).
Result< void > SetServoBaudRate(ServoRef servo, uint32_t bits_per_second) noexcept
Write a servo's Baud Rate register; the new rate applies after the reply. The port keeps its rate: fo...
void GetConfig() const &&=delete
Not on a temporary driver: the reference would dangle (KB-CPP20-044).
Result< void > DisableTorqueAt(uint8_t id, uint16_t torque_address) noexcept
Turn the torque off at an explicit address: the stop for a servo whose family is Generic (a model the...
Result< void > SetStatusReturnLevel(ServoRef servo, StatusReturn level) noexcept
Write the Status Return Level and adopt it as the bus's expectation. A write that lowers the level ma...
DYNAMIXEL & operator=(const DYNAMIXEL &)=delete
One driver object per bus.
Result< void > SetGoalPosition(ServoRef servo, int32_t counts) noexcept
Write the Goal Position.
Result< void > Clear(uint8_t id, ClearOption option) noexcept
Clear the multi-turn count or registered errors (protocol 2.0). Needs Config::allow_destructive.
Result< void > DisableTorqueEach(std::span< const ServoRef > servos) noexcept
DisableTorque() for each servo of a list, whatever one of them does.
Result< PingInfo > Ping(uint8_t id) noexcept
Ping one servo (always answered, whatever the Status Return Level).
Result< void > ApplyConfig(const Config &config) noexcept
Change the whole configuration; the UART rate follows through SetBaudRate() when the port has it.
Result< void > FactoryReset(uint8_t id, ResetOption option) noexcept
Reset a servo's control table to the factory values.
Result< void > ChangeBusBaud(uint32_t bits_per_second) noexcept
Change the UART rate of the port (after SetServoBaudRate()), wait Config::baud_change_wait_us and res...
Result< void > Reboot(uint8_t id) noexcept
Reboot a servo; waits Config::reboot_wait_us after the reply. Sent like the stop: it needs no EnsureI...
Result< void > Deinitialize() noexcept
Mark the driver uninitialised. No bus traffic: the servos keep their state (torque stays on; call Dis...
void GetStats() const &&=delete
Not on a temporary driver (KB-CPP20-044).
void GetLastRequest() const &&=delete
Not on a temporary driver (KB-CPP20-044).
Result< void > EnsureInitialized() noexcept
Start the driver, once: validate the configuration, bring the port up and set its rate.
Result< void > Read(uint8_t id, uint16_t address, std::span< uint8_t > out) noexcept
Read bytes of one servo's control table.
Result< void > DisableTorque(ServoRef servo) noexcept
Turn one servo's torque off. Never refused for the driver's state: it needs no EnsureInitialized() (i...
std::span< const uint8_t > GetLastRequest() const &noexcept
Definition dynamixel.hpp:630
Result< void > FastBulkRead(std::span< const BulkReadItem > items, std::span< uint8_t > out, std::span< DeviceReply > replies) noexcept
Bulk Read with one combined answer packet (Fast Bulk Read, protocol 2.0).
Result< void > FastSyncRead(uint16_t address, uint16_t length, std::span< const uint8_t > ids, std::span< uint8_t > out, std::span< DeviceReply > replies) noexcept
Sync Read with one combined answer packet (Fast Sync Read, protocol 2.0): the servos answer back to b...
Result< void > SetId(ServoRef servo, uint8_t new_id) noexcept
Change a servo's ID. Needs Config::allow_destructive when servo.id is the broadcast ID.
Result< void > WriteRegister(ServoRef servo, Reg reg, int32_t value) noexcept
Encode and write one register.
DYNAMIXEL(const DYNAMIXEL &)=delete
One driver object per bus.
const ReplyInfo & GetLastReply() const &noexcept
Definition dynamixel.hpp:595
Result< uint32_t > ScanIds(uint8_t first, uint8_t last, std::span< PingInfo > found) noexcept
Ping every ID from first to last one after another.
Result< void > Resync() noexcept
Resynchronise: drop everything received and wait until the line has been quiet for one reply timeout,...
Result< void > DisableTorqueAll(Family family) noexcept
Turn the torque of every servo of a family off with one broadcast. Never refused for the driver's sta...
Result< void > Backup(uint8_t id, BackupOption option) noexcept
Store the control table in the backup area, or restore the EEPROM from it (protocol 2....
bool IsInitialized() const noexcept
Definition dynamixel.hpp:197
const Config & GetConfig() const &noexcept
Definition dynamixel.hpp:225
Result< void > BulkRead(std::span< const BulkReadItem > items, std::span< uint8_t > out, std::span< DeviceReply > replies) noexcept
Read different addresses and lengths from several servos (protocol 1.0 for MX and X servos,...
Result< void > Action(uint8_t id) noexcept
Execute the stored Reg Write (all servos at once with the broadcast ID).
const ReplyInfo & GetLatchedAlert() const &noexcept
Definition dynamixel.hpp:603
Result< Feedback > ReadFeedback(ServoRef servo) noexcept
Read position, velocity, load or current, voltage and temperature in one transfer.
Result< int32_t > ReadRegister(ServoRef servo, Reg reg) noexcept
Read one register by its family-neutral name and decode it.
const Stats & GetStats() const &noexcept
Definition dynamixel.hpp:615
void GetLastReply() const &&=delete
Not on a temporary driver (KB-CPP20-044).
Result< uint32_t > BroadcastPing(std::span< PingInfo > found) noexcept
Ping the broadcast ID and collect the answers (protocol 2.0): every servo answers,...
void ClearLatchedAlert() noexcept
Forget the latched alert. No I/O.
Definition dynamixel.hpp:610
Result< void > SetTorque(ServoRef servo, bool enable) noexcept
Turn the torque on or off. Use DisableTorque() to stop: it is never refused.
DYNAMIXEL(Port &port, const Config &config={}) noexcept
Bind the driver to a port and store the configuration. No I/O, cannot fail.
Definition dynamixel.hpp:175
Result< void > Write(uint8_t id, uint16_t address, std::span< const uint8_t > data) noexcept
Write bytes to the control table.
Result< void > BulkWrite(std::span< const BulkWriteItem > items) noexcept
Write different addresses and lengths to several servos with one packet (protocol 2....
Compile-time helpers of the DYNAMIXEL driver: typed integer literals, exact hook matching,...
The packet layer of DYNAMIXEL protocols 1.0 and 2.0: CRC-16, checksum, byte stuffing,...
Control tables of the DYNAMIXEL servo families, the model catalogue and the unit scales.
Public types of the DYNAMIXEL driver: errors, Result<T>, protocol enumerations, replies,...
The UART port of the DYNAMIXEL driver: the CRTP base, the UartPort concept and the line requirements.
ClearOption
The option of a Clear instruction (protocol 2.0).
Definition dynamixel_types.hpp:216
StatusReturn
How much a servo answers (the Status Return Level register, values 0 to 2).
Definition dynamixel_types.hpp:202
Error
Why a call failed. Every call of the driver returns one of these in its Result.
Definition dynamixel_types.hpp:39
ResetOption
The option byte of a Factory Reset instruction (protocol 2.0).
Definition dynamixel_types.hpp:209
LogLevel
Severity passed to the optional Log() hook.
Definition dynamixel_types.hpp:173
BackupOption
The option of a Control Table Backup instruction (protocol 2.0).
Definition dynamixel_types.hpp:222
@ Bus
A port hook returned false.
@ Info
Lifecycle events: initialised, resynchronised.
The DYNAMIXEL driver: everything it declares lives here; internals in detail.
Definition dynamixel.hpp:35
Family
A servo family: one control table layout (several models share it).
Definition dynamixel_tables.hpp:30
Reg
Family-neutral register names.
Definition dynamixel_tables.hpp:67
One device of a Bulk Read request.
Definition dynamixel_types.hpp:315
One device of a Bulk Write request.
Definition dynamixel_types.hpp:322
Everything the driver needs to know about the bus.
Definition dynamixel_types.hpp:385
The per-device outcome of a Sync Read, Bulk Read or Fast read.
Definition dynamixel_types.hpp:307
Position, velocity, effort, voltage and temperature of a servo, read in one transfer.
Definition dynamixel_types.hpp:329
The identity a ping returns.
Definition dynamixel_types.hpp:291
One register of a family's control table.
Definition dynamixel_tables.hpp:156
An error byte, decoded.
Definition dynamixel_types.hpp:282
The value of a call, or why there is none.
Definition dynamixel_types.hpp:113
A servo on the bus: its ID and its control-table family.
Definition dynamixel_types.hpp:300
What the driver counted since ResetStats().
Definition dynamixel_types.hpp:346