diff --git a/components/canopen/example/main/canopen_example.cpp b/components/canopen/example/main/canopen_example.cpp index 175307d5fa..6be1b46927 100644 --- a/components/canopen/example/main/canopen_example.cpp +++ b/components/canopen/example/main/canopen_example.cpp @@ -79,7 +79,7 @@ extern "C" void app_main(void) { .on_heartbeat = // captureless: logger has static storage duration (see above) [](uint8_t hb_node, espp::CanopenClient::NmtState state) { - logger.info("Heartbeat from node {}: NMT state {}", hb_node, static_cast(state)); + logger.info("Heartbeat from node {}: NMT state {}", hb_node, state); }, .log_level = espp::Logger::Verbosity::INFO, }); @@ -129,7 +129,7 @@ extern "C" void app_main(void) { } else { // DS402: profile velocity mode, enable, gentle ramp, stop, disable. if (auto state = drive.get_state(ec); !ec) { - logger.info("Drive state: {}", espp::detail::ds402::state_to_string(state)); + logger.info("Drive state: {}", state); if (state == espp::Ds402Drive::State::Fault) { logger.info("Drive is in Fault; attempting fault reset"); if (!drive.fault_reset(ec)) { diff --git a/components/canopen/include/canopen_client.hpp b/components/canopen/include/canopen_client.hpp index f85c0ac377..f5e0681968 100644 --- a/components/canopen/include/canopen_client.hpp +++ b/components/canopen/include/canopen_client.hpp @@ -12,6 +12,7 @@ #include #include "base_component.hpp" +#include "canopen_format_helpers.hpp" #include "detail/canopen_core.hpp" namespace espp { @@ -460,6 +461,12 @@ class CanopenClient : public BaseComponent { return last_abort_code_; } + /// \brief Human-readable description of a CiA 301 SDO abort code (e.g. for + /// logging the reason behind an SDO failure / last_abort_code()). + static const char *abort_code_to_string(uint32_t abort_code) { + return detail::canopen::sdo_abort_to_string(abort_code); + } + /// @} protected: diff --git a/components/canopen/include/canopen_format_helpers.hpp b/components/canopen/include/canopen_format_helpers.hpp new file mode 100644 index 0000000000..216b1152f7 --- /dev/null +++ b/components/canopen/include/canopen_format_helpers.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include + +#include "format.hpp" + +#include "detail/canopen_core.hpp" + +// libfmt formatters for the CANopen / CiA 402 enums so application code can print +// them directly -- e.g. `logger.info("Drive state: {}", state)` -- instead of +// calling a to-string helper inline at every call site. + +/// \brief fmt formatter for a CiA 402 drive state (espp::Ds402Drive::State). +template <> struct fmt::formatter : fmt::formatter { + template + auto format(espp::detail::ds402::State state, FormatContext &ctx) const { + return fmt::formatter::format(espp::detail::ds402::state_to_string(state), + ctx); + } +}; + +/// \brief fmt formatter for a CiA 402 mode of operation +/// (espp::Ds402Drive::OperatingMode). +template <> +struct fmt::formatter : fmt::formatter { + template + auto format(espp::detail::ds402::OperatingMode mode, FormatContext &ctx) const { + using espp::detail::ds402::OperatingMode; + std::string_view s = "Unknown"; + switch (mode) { + case OperatingMode::ProfilePosition: + s = "Profile position"; + break; + case OperatingMode::ProfileVelocity: + s = "Profile velocity"; + break; + case OperatingMode::ProfileTorque: + s = "Profile torque"; + break; + case OperatingMode::Homing: + s = "Homing"; + break; + } + return fmt::formatter::format(s, ctx); + } +}; + +/// \brief fmt formatter for an NMT node state (espp::CanopenClient::NmtState). +template <> +struct fmt::formatter : fmt::formatter { + template + auto format(espp::detail::canopen::NmtState state, FormatContext &ctx) const { + using espp::detail::canopen::NmtState; + std::string_view s = "Unknown"; + switch (state) { + case NmtState::BootUp: + s = "Boot-up"; + break; + case NmtState::Stopped: + s = "Stopped"; + break; + case NmtState::Operational: + s = "Operational"; + break; + case NmtState::PreOperational: + s = "Pre-operational"; + break; + case NmtState::Unknown: + break; + } + return fmt::formatter::format(s, ctx); + } +}; + +/// \brief fmt formatter for an NMT master command (espp::CanopenClient::NmtCommand). +template <> +struct fmt::formatter : fmt::formatter { + template + auto format(espp::detail::canopen::NmtCommand cmd, FormatContext &ctx) const { + using espp::detail::canopen::NmtCommand; + std::string_view s = "Unknown"; + switch (cmd) { + case NmtCommand::Start: + s = "Start"; + break; + case NmtCommand::Stop: + s = "Stop"; + break; + case NmtCommand::PreOperational: + s = "Pre-operational"; + break; + case NmtCommand::ResetNode: + s = "Reset node"; + break; + case NmtCommand::ResetCommunication: + s = "Reset communication"; + break; + } + return fmt::formatter::format(s, ctx); + } +}; diff --git a/components/canopen/include/detail/canopen_core.hpp b/components/canopen/include/detail/canopen_core.hpp index a747cecde5..bf1cf33235 100644 --- a/components/canopen/include/detail/canopen_core.hpp +++ b/components/canopen/include/detail/canopen_core.hpp @@ -462,13 +462,15 @@ inline constexpr uint16_t OBJ_CONTROLWORD = 0x6040; ///< Controlword (u16 inline constexpr uint16_t OBJ_STATUSWORD = 0x6041; ///< Statusword (u16). inline constexpr uint16_t OBJ_MODES_OF_OPERATION = 0x6060; ///< Modes of operation (i8). inline constexpr uint16_t OBJ_MODES_OF_OPERATION_DISPLAY = 0x6061; ///< Modes display (i8). -inline constexpr uint16_t OBJ_POSITION_ACTUAL = 0x6064; ///< Position actual value (i32). -inline constexpr uint16_t OBJ_VELOCITY_ACTUAL = 0x606C; ///< Velocity actual value (i32). -inline constexpr uint16_t OBJ_TARGET_POSITION = 0x607A; ///< Target position (i32). -inline constexpr uint16_t OBJ_PROFILE_VELOCITY = 0x6081; ///< Profile velocity (u32). -inline constexpr uint16_t OBJ_PROFILE_ACCELERATION = 0x6083; ///< Profile acceleration (u32). -inline constexpr uint16_t OBJ_PROFILE_DECELERATION = 0x6084; ///< Profile deceleration (u32). -inline constexpr uint16_t OBJ_TARGET_VELOCITY = 0x60FF; ///< Target velocity (i32). +inline constexpr uint16_t OBJ_SOFTWARE_POSITION_LIMIT = + 0x607D; ///< Software position limit (i32; subindex 1 = min, 2 = max). +inline constexpr uint16_t OBJ_POSITION_ACTUAL = 0x6064; ///< Position actual value (i32). +inline constexpr uint16_t OBJ_VELOCITY_ACTUAL = 0x606C; ///< Velocity actual value (i32). +inline constexpr uint16_t OBJ_TARGET_POSITION = 0x607A; ///< Target position (i32). +inline constexpr uint16_t OBJ_PROFILE_VELOCITY = 0x6081; ///< Profile velocity (u32). +inline constexpr uint16_t OBJ_PROFILE_ACCELERATION = 0x6083; ///< Profile acceleration (u32). +inline constexpr uint16_t OBJ_PROFILE_DECELERATION = 0x6084; ///< Profile deceleration (u32). +inline constexpr uint16_t OBJ_TARGET_VELOCITY = 0x60FF; ///< Target velocity (i32). /// @} /// @name Object-index bounds used for per-axis offsetting diff --git a/components/canopen/include/ds402.hpp b/components/canopen/include/ds402.hpp index e0123e6924..4d9cc18e15 100644 --- a/components/canopen/include/ds402.hpp +++ b/components/canopen/include/ds402.hpp @@ -34,6 +34,18 @@ class Ds402Drive : public BaseComponent { using State = detail::ds402::State; ///< CiA 402 drive state. using OperatingMode = detail::ds402::OperatingMode; ///< CiA 402 mode of operation. + /// \brief Human-readable name for a CiA 402 drive state (e.g. for logging). + /// \return A pointer to a string literal with static lifetime — do not free it; + /// it stays valid for the program's duration. + static const char *to_string(State state) { return detail::ds402::state_to_string(state); } + + /// \brief Decode a raw CiA 402 statusword (object 0x6041) into a drive state. + /// \details Useful for decoding a statusword you already have (e.g. from a + /// cached read or a TPDO) without another SDO round-trip. + static State state_from_statusword(uint16_t statusword) { + return detail::ds402::decode_state(statusword); + } + /// \brief Configuration for the Ds402Drive. struct Config { std::chrono::milliseconds state_timeout{ diff --git a/components/mcp266/include/mcp266.hpp b/components/mcp266/include/mcp266.hpp index 0d5e09aff8..240405d337 100644 --- a/components/mcp266/include/mcp266.hpp +++ b/components/mcp266/include/mcp266.hpp @@ -194,15 +194,19 @@ class Mcp266 : public BaseComponent { } /// \brief Set the CiA 402 software position limits (0x607D:1/:2) for an axis. - /// \param axis The motor channel. \param min_pos Lower limit. \param max_pos - /// Upper limit. \param ec Set on failure. \return True on success. + /// \param axis The motor channel. + /// \param min_pos Lower limit. + /// \param max_pos Upper limit. + /// \param ec Set on failure. + /// \return True on success. bool set_position_limits(Axis axis, int32_t min_pos, int32_t max_pos, std::error_code &ec) { ec.clear(); if (min_pos > max_pos) { ec = std::make_error_code(std::errc::invalid_argument); return false; } - const uint16_t obj = static_cast(0x607D + axis_state(axis).objects.object_offset); + const uint16_t obj = detail::ds402::apply_axis_offset( + detail::ds402::OBJ_SOFTWARE_POSITION_LIMIT, axis_state(axis).objects.object_offset); return client_.write_i32(obj, 1, min_pos, ec) && client_.write_i32(obj, 2, max_pos, ec); } @@ -263,42 +267,76 @@ class Mcp266 : public BaseComponent { /// @name Feedback /// @{ - /// \brief Read the actual position (0x6064 / 0x6864). \param axis Channel. - /// \param count Out: encoder counts. \param ec Set on failure. \return True on success. + /// \brief Read the actual position (0x6064 / 0x6864). + /// \param axis Channel. + /// \param count Out: encoder counts. + /// \param ec Set on failure. + /// \return True on success. bool read_encoder(Axis axis, int32_t &count, std::error_code &ec) { ec.clear(); count = axis_state(axis).drive.get_position_actual(ec); return !ec; } - /// \brief Read the actual velocity (0x606C / 0x686C). \param axis Channel. - /// \param qpps Out: counts/s. \param ec Set on failure. \return True on success. + /// \brief Read the actual velocity (0x606C / 0x686C). + /// \param axis Channel. + /// \param qpps Out: counts/s. + /// \param ec Set on failure. + /// \return True on success. bool read_speed(Axis axis, int32_t &qpps, std::error_code &ec) { ec.clear(); qpps = axis_state(axis).drive.get_velocity_actual(ec); return !ec; } - /// \brief Read the CiA 402 statusword (0x6041 / 0x6841). \param axis Channel. - /// \param statusword Out. \param ec Set on failure. \return True on success. + /// \brief Read the CiA 402 statusword (0x6041 / 0x6841). + /// \param axis Channel. + /// \param statusword Out. + /// \param ec Set on failure. + /// \return True on success. bool read_statusword(Axis axis, uint16_t &statusword, std::error_code &ec) { ec.clear(); statusword = axis_state(axis).drive.get_statusword(ec); return !ec; } + /// \brief Read the decoded CiA 402 drive state of an axis (from its statusword). + /// \param axis Channel. + /// \param state Out: the power-drive-system state. + /// \param ec Set on failure. + /// \return True on success. + bool get_state(Axis axis, Ds402Drive::State &state, std::error_code &ec) { + ec.clear(); + state = axis_state(axis).drive.get_state(ec); + return !ec; + } + /// \brief Whether an axis reports "target reached" (statusword bit 10) — the + /// authoritative arrival signal for profile moves. + /// \param axis Channel. + /// \param reached Out. + /// \param ec Set on failure. + /// \return True on success. + bool is_target_reached(Axis axis, bool &reached, std::error_code &ec) { + ec.clear(); + reached = axis_state(axis).drive.is_target_reached(ec); + return !ec; + } /// @} /// @name Device telemetry /// @{ - /// \brief Read the main battery voltage (mirrored command 24). \param volts - /// Out: volts. \param ec Set on failure. \return True on success. + /// \brief Read the main battery voltage (mirrored command 24). + /// \param volts Out: volts. + /// \param ec Set on failure. + /// \return True on success. bool read_main_battery_voltage(float &volts, std::error_code &ec) { ec.clear(); volts = static_cast(client_.read_u16(detail::mcp266::kMainBatteryObject, 0, ec)) / 10.0f; return !ec; } - /// \brief Read the board temperature (mirrored command 82). \param temp_c - /// Out: degrees C. \param ec Set on failure. \return True on success. + /// \brief Read the board temperature (mirrored command 82). + /// \param temp_c Out: degrees C. + /// \param ec Set on failure. + /// \return True on success. bool read_temperature(float &temp_c, std::error_code &ec) { ec.clear(); temp_c = @@ -319,15 +357,22 @@ class Mcp266 : public BaseComponent { /// @} /// \brief Access an axis's underlying Ds402Drive for advanced CiA 402 use. - /// \param axis The motor channel. \return Reference to the axis drive helper. + /// \param axis The motor channel. + /// \return Reference to the axis drive helper. Ds402Drive &drive(Axis axis) { return axis_state(axis).drive; } private: /// Coarse fallback position P gain, used only when the drive's stored gain /// reads back as zero (see configure_position_loop()). It is a non-tuned /// starting point that produces motion out of the box, not a good gain for - /// any particular motor; callers should tune and pass their own. - static constexpr int32_t kDefaultPositionP = 0x3C83; + /// any particular motor; callers should tune and pass their own. 15491 is + /// ~15.1 in the MCP's position-PID fixed-point representation (x1024). + static constexpr int32_t kDefaultPositionP = 15491; // = 0x3C83 + + /// The MCP266 does not echo the requested mode in 0x6061, so after writing the + /// mode of operation enable() waits this fixed settle time before reading the + /// state rather than polling the (unchanging) mode display. + static constexpr auto kModeSettle = std::chrono::milliseconds(25); /// Per-axis state: the manufacturer object addresses and a Ds402Drive whose /// object offset selects M1 (0) or M2 (0x800). @@ -353,12 +398,13 @@ class Mcp266 : public BaseComponent { /// display -- would time out), clear any fault, and walk to Operation /// Enabled. bool enable(AxisState &a, Ds402Drive::OperatingMode mode, std::error_code &ec) { - const uint16_t mode_obj = static_cast(0x6060 + a.objects.object_offset); + const uint16_t mode_obj = detail::ds402::apply_axis_offset( + detail::ds402::OBJ_MODES_OF_OPERATION, a.objects.object_offset); if (!client_.write_i8(mode_obj, 0, static_cast(mode), ec)) { logger_.error("{}: failed to set mode: {}", a.name, ec.message()); return false; } - std::this_thread::sleep_for(std::chrono::milliseconds(25)); + std::this_thread::sleep_for(kModeSettle); const auto state = a.drive.get_state(ec); if (ec) { return false; diff --git a/doc/Doxyfile b/doc/Doxyfile index ba074b4d14..81de57f18c 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -257,6 +257,7 @@ INPUT = \ $(PROJECT_PATH)/components/button/include/button.hpp \ $(PROJECT_PATH)/components/byte90/include/byte90.hpp \ $(PROJECT_PATH)/components/canopen/include/canopen_client.hpp \ + $(PROJECT_PATH)/components/canopen/include/canopen_format_helpers.hpp \ $(PROJECT_PATH)/components/canopen/include/detail/canopen_core.hpp \ $(PROJECT_PATH)/components/canopen/include/ds402.hpp \ $(PROJECT_PATH)/components/chsc6x/include/chsc6x.hpp \