33#include "dynamixel_version.h"
44template <
typename Port>
45concept DeclaresSetBaudRate =
requires(Port& port) { port.SetBaudRate(1_u32); };
48template <
typename Port>
49concept HasSetBaudRate =
50 requires { ExactHook<Port, bool(uint32_t)
noexcept>::Take(&Port::SetBaudRate); };
53template <
typename Port>
54concept DeclaresLog =
requires(Port& port) { port.Log(
LogLevel::Info,
""); };
57template <
typename Port>
59 requires { ExactHook<Port, void(
LogLevel,
const char*)
noexcept>::Take(&Port::Log); };
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);
68 static_cast<void>(port);
79 std::span<const uint8_t> ids;
80 std::span<const BulkReadItem> items;
83 [[nodiscard]] std::size_t Count()
const noexcept {
84 return bulk ? items.size() : ids.size();
87 [[nodiscard]] uint8_t IdAt(std::size_t k)
const noexcept {
88 return bulk ? items[k].id : ids[k];
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);
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);
99 [[nodiscard]]
bool ListedAfter(std::size_t k, uint8_t
id)
const noexcept {
101 for (std::size_t j = k + 1U; j < Count(); ++j) {
102 later = later || (IdAt(j) == id);
121 bool confirmed{
false};
122 bool flush_fault{
false};
158template <UartPort Port, std::
size_t FrameBytes = 512>
160 static_assert((FrameBytes >= 64U) && (FrameBytes <= 4096U),
161 "dynamixel: FrameBytes must be 64 to 4096");
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) "
176 : port_(port), config_(config) {}
294 Result<
void>
Read(uint8_t
id, uint16_t address, std::span<uint8_t> out) noexcept;
307 Result<
void>
Write(uint8_t
id, uint16_t address, std::span<const uint8_t> data) noexcept;
316 Result<
void>
RegWrite(uint8_t
id, uint16_t address, std::span<const uint8_t> data) noexcept;
393 Result<
void>
SyncWrite(uint16_t address, uint16_t length, std::span<const uint8_t> ids,
394 std::span<const uint8_t> data) noexcept;
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;
433 std::span<uint8_t> out, std::span<
DeviceReply> replies) noexcept;
604 return latched_alert_;
631 return std::span<const uint8_t>{tx_.data(), tx_len_};
640 enum class ReplyRule : uint8_t {
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;
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;
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,
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,
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,
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,
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,
689 Result<
void> applyRate() noexcept;
690 Result<
void> fail(
Error error, const
char* message) noexcept;
691 void noteReply(const
ReplyInfo& info) noexcept;
694 static constexpr std::
size_t kScratchBytes = 64;
696 static constexpr uint32_t kStopAttempts = 3;
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_{};
710 std::size_t rx_drop_{
713 bool initialized_{
false};
721#include "../src/dynamixel.ipp"
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