diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 76689b2f..c3081330 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,6 +1,10 @@ add_library(motor_core INTERFACE) -target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices) +target_include_directories(motor_core + INTERFACE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/devices +) target_link_libraries(motor_core INTERFACE @@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core) add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) +add_subdirectory(drivers/ethercat_motor) add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt new file mode 100644 index 00000000..8c7d93df --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt @@ -0,0 +1,49 @@ +add_library(ethercat_motor_driver SHARED + src/cia402/cia402_protocol.cpp + src/vendor/eyou/eyou_motor.cpp + src/vendor/eyou/eyou_motor_adapter.cpp +) + +target_include_directories(ethercat_motor_driver + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(ethercat_motor_driver + PUBLIC + cmvr_es::device::motor_core + cmvr_es::device::motor_bus_runtime + PRIVATE + cmvr_es::proto + glog +) + +add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver) +install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib) + +add_executable(eyou_motor_real_test + src/vendor/eyou/eyou_motor_real_test.cpp +) + +target_link_libraries(eyou_motor_real_test + PRIVATE + cmvr_es::device::ethercat_motor_driver + gtest + gtest_main + pthread + glog +) + +add_executable(eyou_motor_device_manager_real_test + src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +) + +target_link_libraries(eyou_motor_device_manager_real_test + PRIVATE + cmvr_es::device_manager + cmvr_es::device::motor_manager + gtest + gtest_main + pthread + glog +) diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h new file mode 100644 index 00000000..e3e92dcb --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h @@ -0,0 +1,161 @@ +#ifndef CMVR_ES_CIA402_OBJECTS_H +#define CMVR_ES_CIA402_OBJECTS_H + +#include + +namespace cmvr::device::cia402 { + +union Controlword { + std::uint16_t value; + struct { + std::uint16_t switch_on : 1; + std::uint16_t enable_voltage : 1; + std::uint16_t quick_stop : 1; + std::uint16_t enable_operation : 1; + std::uint16_t new_set_point : 1; + std::uint16_t change_set_immediately : 1; + std::uint16_t relative : 1; + std::uint16_t fault_reset : 1; + std::uint16_t halt : 1; + std::uint16_t reserved : 2; + std::uint16_t manufacturer_specific : 5; + }; +}; + +union Statusword { + std::uint16_t value; + struct { + std::uint16_t ready_to_switch_on : 1; + std::uint16_t switched_on : 1; + std::uint16_t operation_enabled : 1; + std::uint16_t fault : 1; + std::uint16_t voltage_enabled : 1; + std::uint16_t quick_stop : 1; + std::uint16_t switch_on_disabled : 1; + std::uint16_t warning : 1; + std::uint16_t manufacturer_specific_8 : 1; + std::uint16_t remote : 1; + std::uint16_t target_reached : 1; + std::uint16_t internal_limit_active : 1; + std::uint16_t operation_mode_specific : 2; + std::uint16_t manufacturer_specific : 2; + }; +}; + +static_assert(sizeof(Controlword) == sizeof(std::uint16_t)); +static_assert(sizeof(Statusword) == sizeof(std::uint16_t)); + +enum class DeviceState { + SwitchOnDisabled, + ReadyToSwitchOn, + SwitchedOn, + OperationEnabled, +}; + +namespace detail { + +struct StateRule { + std::uint16_t relevant_bits; + std::uint16_t expected_bits; +}; + +inline StateRule stateRule(const DeviceState state) +{ + // CiA402 device states are matched by selected 0x6041 statusword bits. + switch (state) { + case DeviceState::SwitchOnDisabled: + return {0x004F, 0x0040}; + case DeviceState::ReadyToSwitchOn: + return {0x006F, 0x0021}; + case DeviceState::SwitchedOn: + return {0x006F, 0x0023}; + case DeviceState::OperationEnabled: + return {0x006F, 0x0027}; + } + return {0x006F, 0x0000}; +} + +} // namespace detail + +inline Controlword controlword(const std::uint16_t value) +{ + Controlword cw{}; + cw.value = value; + return cw; +} + +inline Statusword statusword(const std::uint16_t value) +{ + Statusword sw{}; + sw.value = value; + return sw; +} + +inline Controlword shutdownControlword() +{ + Controlword cw{}; + cw.quick_stop = 1; + cw.enable_voltage = 1; + return cw; +} + +inline Controlword switchOnControlword() +{ + auto cw = shutdownControlword(); + cw.switch_on = 1; + return cw; +} + +inline Controlword enableOperationControlword() +{ + auto cw = switchOnControlword(); + cw.enable_operation = 1; + return cw; +} + +inline Controlword quickStopControlword() +{ + auto cw = enableOperationControlword(); + cw.quick_stop = 0; + return cw; +} + +inline Controlword faultResetControlword() +{ + Controlword cw{}; + cw.fault_reset = 1; + return cw; +} + +inline Controlword profilePositionControlword(const bool new_set_point) +{ + auto cw = enableOperationControlword(); + cw.change_set_immediately = 1; + cw.new_set_point = new_set_point ? 1 : 0; + return cw; +} + +inline bool hasState(const Statusword status, const DeviceState state) +{ + const auto rule = detail::stateRule(state); + return (status.value & rule.relevant_bits) == rule.expected_bits; +} + +inline bool isSwitchOnDisabled(const Statusword status) +{ + return hasState(status, DeviceState::SwitchOnDisabled); +} + +inline bool isOperationEnabled(const Statusword status) +{ + return hasState(status, DeviceState::OperationEnabled); +} + +inline bool targetReached(const Statusword status) +{ + return status.target_reached != 0; +} + +} // namespace cmvr::device::cia402 + +#endif // CMVR_ES_CIA402_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h new file mode 100644 index 00000000..c4cc46b5 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h @@ -0,0 +1,116 @@ +#ifndef CMVR_ES_CIA402_PROTOCOL_H +#define CMVR_ES_CIA402_PROTOCOL_H + +#include +#include +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" +#include "devices/motor/motor_protocol_interface.h" + +namespace cmvr::device { + +class Cia402Protocol final : public MotorProtocolInterface { +public: + explicit Cia402Protocol(std::shared_ptr bus_runtime, + const config::Cia402ProtocolConfig& config); + ~Cia402Protocol() override = default; + + bool initNode(std::uint8_t node_id) override; + + void setMode(std::uint8_t node_id, msgs::RunMode mode) override; + msgs::RunMode getMode(std::uint8_t node_id) override; + void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override; + void setLimitQd(std::uint8_t node_id, double qd) override; + void setLimitQ(std::uint8_t node_id, double ub, double lb) override; + bool calibrateZeroQ(std::uint8_t node_id) override; + bool reachedTargetQ(std::uint8_t node_id) override; + bool commandProfilePosition(std::uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(std::uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(std::uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicVelocity(std::uint8_t node_id, + double target_qd) override; + bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; + void setMotorConversion(std::uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; + bool torqueOn(std::uint8_t node_id) override; + bool torqueOff(std::uint8_t node_id) override; + bool brakeRelease(std::uint8_t node_id) override; + bool quickStop(std::uint8_t node_id) override; + + double getQ(std::uint8_t node_id) override; + double getQd(std::uint8_t node_id) override; + bool syncTargetToActualPosition(std::uint8_t node_id); + +private: + struct NodeState { + msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION}; + cia402::Controlword controlword{}; + std::int32_t target_position{0}; + std::int32_t target_velocity{0}; + std::int16_t target_torque{0}; + std::int32_t profile_velocity{0}; + std::int32_t profile_acceleration{0}; + std::int32_t profile_deceleration{0}; + double limit_q_lb{0.0}; + double limit_q_ub{0.0}; + double limit_qd{0.0}; + double limit_qdd{0.0}; + double encoder_counts_per_rev{0.0}; + double gear_ratio{0.0}; + }; + + static std::int8_t toCia402Mode_(msgs::RunMode mode); + static msgs::RunMode fromCia402Mode_(std::int8_t mode); + static cia402::Controlword nextControlword_(cia402::Statusword statusword); + static bool isOperationEnabled_(cia402::Statusword statusword); + static bool targetReached_(cia402::Statusword statusword); + + std::int32_t radToCounts_(double angle_rad, const NodeState& state) const; + double countsToRad_(std::int32_t counts, const NodeState& state) const; + std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const; + std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const; + double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const; + NodeState& nodeState_(std::uint8_t node_id); + const NodeState* findNodeState_(std::uint8_t node_id) const; + bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const; + bool validateNodePdos_(std::uint8_t node_id) const; + bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const; + bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const; + bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const; + bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const; + bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword); + bool writeControlwordAndWait_(std::uint8_t node_id, + cia402::Controlword controlword, + cia402::DeviceState target_state, + const char* state_name); + bool waitStatus_(std::uint8_t node_id, + cia402::DeviceState target_state, + const char* state_name) const; + bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const; + bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state); + void writeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, const NodeState& state) const; + void writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state); + void writeNode_(std::uint8_t node_id, NodeState& state); + + std::shared_ptr bus_runtime_; + config::Cia402ProtocolConfig config_; + std::unordered_map nodes_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CIA402_PROTOCOL_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h new file mode 100644 index 00000000..0993dd57 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h @@ -0,0 +1,81 @@ +#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H +#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H + +#include +#include +#include + +#include "cmvr/msgs/canopen.pb.h" +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h" + +namespace cmvr::device { + +namespace eyou_cia402_pdo_mapping_detail { + +inline constexpr std::uint32_t VENDOR_ID = 0x00001097; +inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406; + +inline EthercatPdoEntryConfig entry(const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::string name) +{ + EthercatPdoEntryConfig cfg; + cfg.index = index; + cfg.subindex = subindex; + cfg.bit_len = bit_len; + cfg.name = std::move(name); + cfg.padding = index == 0 || bit_len == 0; + return cfg; +} + +} // namespace eyou_cia402_pdo_mapping_detail + +inline EthercatPdoMapping createEyouCia402PdoMapping() +{ + using namespace eyou_cia402_pdo_mapping_detail; + + EthercatPdoConfig rx_pdo; + rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601; + rx_pdo.sync_manager = 2; + rx_pdo.rx = true; + rx_pdo.entries = { + entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"), + entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"), + entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"), + entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"), + entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"), + entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"), + entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"), + entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"), + entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"), + entry(0x0000, 0x00, 8, "Padding"), + }; + + EthercatPdoConfig tx_pdo; + tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00; + tx_pdo.sync_manager = 3; + tx_pdo.rx = false; + tx_pdo.entries = { + entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"), + entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"), + entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"), + entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"), + entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"), + entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"), + entry(0x0000, 0x00, 8, "Padding"), + }; + + EthercatPdoMapping mapping; + mapping.vendor_id = VENDOR_ID; + mapping.product_code = PRODUCT_CODE; + mapping.name = "EYOU ServoModule ECAT V145 CiA402"; + mapping.rx_pdos.push_back(std::move(rx_pdo)); + mapping.tx_pdos.push_back(std::move(tx_pdo)); + return mapping; +} + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h new file mode 100644 index 00000000..10f3ee96 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h @@ -0,0 +1,43 @@ +#ifndef CMVR_ES_EYOU_MOTOR_H +#define CMVR_ES_EYOU_MOTOR_H + +#include +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +namespace cmvr::device { + +class EyouMotor final : public AbstractMotor { +public: + EyouMotor(const config::MotorConfigItem& config, + std::shared_ptr cia402_protocol, + std::unique_ptr vendor_adapter); + + std::string typeName() const override { return "EyouMotor"; } + bool init() override; + void setLimitQ(double ub, double lb) override; + void setLimitQd(double qd) override; + bool calibrateZeroQ() override; + bool brakeRelease() override; + +private: + bool hasDependencies_() const; + bool hasValidConversion_() const; + bool writeVendorPositionLimits_() const; + bool writeVendorVelocityLimit_() const; + std::int32_t radToCounts_(double angle_rad) const; + std::uint32_t radPerSecToCounts_(double velocity_rad_s) const; + + std::shared_ptr cia402_protocol_; + std::unique_ptr vendor_adapter_; + double encoder_counts_per_rev_{0.0}; + double gear_ratio_{0.0}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_MOTOR_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h new file mode 100644 index 00000000..a9d95490 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h @@ -0,0 +1,33 @@ +#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H +#define CMVR_ES_EYOU_MOTOR_ADAPTER_H + +#include +#include + +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h" + +namespace cmvr::device { + +class EyouMotorAdapter final : public MotorVendorAdapter { +public: + explicit EyouMotorAdapter(std::shared_ptr bus_runtime); + ~EyouMotorAdapter() override = default; + + bool initNode(std::uint8_t node_id) override; + bool writePositionLimits(std::uint8_t node_id, + std::int32_t lower_limit, + std::int32_t upper_limit) override; + bool writeVelocityLimit(std::uint8_t node_id, + std::uint32_t velocity_limit) override; + bool calibrateZero(std::uint8_t node_id, + std::int32_t& zeroed_position) override; + bool brakeRelease(std::uint8_t node_id) override; + +private: + std::shared_ptr bus_runtime_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h new file mode 100644 index 00000000..cee1369d --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h @@ -0,0 +1,20 @@ +#ifndef CMVR_ES_EYOU_OBJECTS_H +#define CMVR_ES_EYOU_OBJECTS_H + +#include + +namespace cmvr::device::eyou { + +inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003; + +inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014; + +inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024; +inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A; +inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B; + +inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010; + +} // namespace cmvr::device::eyou + +#endif // CMVR_ES_EYOU_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h new file mode 100644 index 00000000..5af59549 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h @@ -0,0 +1,25 @@ +#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H +#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H + +#include + +namespace cmvr::device { + +class MotorVendorAdapter { +public: + virtual ~MotorVendorAdapter() = default; + + virtual bool initNode(std::uint8_t node_id) = 0; + virtual bool writePositionLimits(std::uint8_t node_id, + std::int32_t lower_limit, + std::int32_t upper_limit) = 0; + virtual bool writeVelocityLimit(std::uint8_t node_id, + std::uint32_t velocity_limit) = 0; + virtual bool calibrateZero(std::uint8_t node_id, + std::int32_t& zeroed_position) = 0; + virtual bool brakeRelease(std::uint8_t node_id) = 0; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp new file mode 100644 index 00000000..7dbd8e65 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp @@ -0,0 +1,872 @@ +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" + +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" + +namespace cmvr::device { + +Cia402Protocol::Cia402Protocol(std::shared_ptr bus_runtime, + const config::Cia402ProtocolConfig& config) + : bus_runtime_(std::move(bus_runtime)), + config_(config) +{ + comm_proto = CommProto::ETHERCAT; +} + +bool Cia402Protocol::initNode(const std::uint8_t node_id) +{ + if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] missing EtherCAT motor: " + << static_cast(node_id); + return false; + } + + if (!validateNodePdos_(node_id)) { + return false; + } + auto& state = nodeState_(node_id); + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id, + const double target_q, + const double max_qd, + const double max_qdd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_position = radToCounts_(target_q, state); + state.profile_velocity = std::abs(radPerSecToCounts_(max_qd, state)); + if (max_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + writeProfilePositionTarget_(node_id, state); + return true; +} + +bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id, + const double target_qd, + const double max_qdd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_velocity = radPerSecToCounts_(target_qd, state); + if (max_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id, + const double target_q, + const double target_qd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_position = radToCounts_(target_q, state); + state.target_velocity = radPerSecToCounts_(target_qd, state); + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id, + const double target_qd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_velocity = radPerSecToCounts_(target_qd, state); + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id, + const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[Cia402Protocol] cyclic torque command is not implemented, node=" + << static_cast(node_id); + return false; +} + +void Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode) +{ + auto& state = nodeState_(node_id); + if (!prepareSafeTargetsForMode_(node_id, mode, state)) { + return; + } + writeTargetsForMode_(node_id, mode, state); + state.mode = mode; + writeNode_(node_id, state); +} + +msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id) +{ + std::int8_t mode_display = 0; + if (readModeDisplay_(node_id, mode_display)) { + return fromCia402Mode_(mode_display); + } + return nodeState_(node_id).mode; +} + +void Cia402Protocol::setLimitQdd(const std::uint8_t node_id, + const double u_qdd, + const double l_qdd) +{ + auto& state = nodeState_(node_id); + state.limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd)); + if (hasValidConversion_(node_id, state)) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + writeAccelerationLimitsToDictionary_(node_id, state); + } +} + +void Cia402Protocol::setLimitQd(const std::uint8_t node_id, const double qd) +{ + auto& state = nodeState_(node_id); + state.limit_qd = std::abs(qd); + if (hasValidConversion_(node_id, state)) { + state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state)); + writeVelocityLimitToDictionary_(node_id, state); + } +} + +void Cia402Protocol::setLimitQ(const std::uint8_t node_id, + const double ub, + const double lb) +{ + auto& state = nodeState_(node_id); + state.limit_q_ub = ub; + state.limit_q_lb = lb; + if (hasValidConversion_(node_id, state)) { + writePositionLimitsToDictionary_(node_id, state); + } +} + +bool Cia402Protocol::calibrateZeroQ(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[Cia402Protocol] zero calibration is vendor-specific, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::reachedTargetQ(const std::uint8_t node_id) +{ + std::uint16_t statusword = 0; + if (!readStatusword_(node_id, statusword)) { + return false; + } + return targetReached_(cia402::statusword(statusword)); +} + +void Cia402Protocol::setMotorConversion( + const std::uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) +{ + auto& state = nodeState_(node_id); + state.encoder_counts_per_rev = encoder_counts_per_rev; + state.gear_ratio = gear_ratio; +} + +bool Cia402Protocol::torqueOn(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + auto& state = nodeState_(node_id); + std::uint16_t statusword = 0; + if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) { + if (!writeControlword_(node_id, cia402::faultResetControlword())) { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } + + if (!prepareSafeTargetsForMode_(node_id, msgs::RUN_MODE_PROFILE_POSITION, state)) { + return false; + } + state.mode = msgs::RUN_MODE_PROFILE_POSITION; + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + if (!bus_runtime_->writePdo(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, + toCia402Mode_(state.mode))) { + return false; + } + + if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::enableOperationControlword(), + cia402::DeviceState::OperationEnabled, + "Operation Enabled")) { + return false; + } + + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + writeTargetsForMode_(node_id, state.mode, state); + + state.controlword = cia402::profilePositionControlword(true); + if (!writeControlword_(node_id, state.controlword)) { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.profile_position_trigger_delay_ms())); + + state.controlword = cia402::profilePositionControlword(false); + return writeControlword_(node_id, state.controlword); +} + +bool Cia402Protocol::torqueOff(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + auto& state = nodeState_(node_id); + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + waitVelocityNearZero_(node_id, "torqueOff"); + + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On")) { + return false; + } + + return true; +} + +bool Cia402Protocol::brakeRelease(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[Cia402Protocol] brake release is vendor-specific, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::quickStop(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + auto& state = nodeState_(node_id); + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + + state.controlword = cia402::quickStopControlword(); + if (!writeControlword_(node_id, state.controlword)) { + return false; + } + return waitVelocityNearZero_(node_id, "quickStop"); +} + +double Cia402Protocol::getQ(const std::uint8_t node_id) +{ + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + return 0.0; + } + const auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return 0.0; + } + return countsToRad_(actual_position, state); +} + +double Cia402Protocol::getQd(const std::uint8_t node_id) +{ + std::int32_t actual_velocity = 0; + if (!readActualVelocity_(node_id, actual_velocity)) { + return 0.0; + } + const auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return 0.0; + } + return countsToRadPerSec_(actual_velocity, state); +} + +bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id) +{ + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual position, node=" + << static_cast(node_id); + return false; + } + + auto& state = nodeState_(node_id); + state.target_position = actual_position; + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + return true; +} + +std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode) +{ + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + return 1; + case msgs::RUN_MODE_PROFILE_VELOCITY: + return 3; + case msgs::RUN_MODE_HOMING: + return 6; + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + return 8; + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + return 9; + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + return 10; + default: + return 8; + } +} + +msgs::RunMode Cia402Protocol::fromCia402Mode_(const std::int8_t mode) +{ + switch (mode) { + case 1: + return msgs::RUN_MODE_PROFILE_POSITION; + case 3: + return msgs::RUN_MODE_PROFILE_VELOCITY; + case 6: + return msgs::RUN_MODE_HOMING; + case 8: + return msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + case 9: + return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + case 10: + return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT; + default: + return msgs::RUN_MODE_UNSPECIFIED; + } +} + +cia402::Controlword Cia402Protocol::nextControlword_(const cia402::Statusword statusword) +{ + if (statusword.fault != 0) { + return cia402::faultResetControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::SwitchOnDisabled)) { + return cia402::shutdownControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::ReadyToSwitchOn)) { + return cia402::switchOnControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::SwitchedOn)) { + return cia402::enableOperationControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::OperationEnabled)) { + return cia402::enableOperationControlword(); + } + return cia402::shutdownControlword(); +} + +bool Cia402Protocol::isOperationEnabled_(const cia402::Statusword statusword) +{ + return cia402::isOperationEnabled(statusword); +} + +bool Cia402Protocol::targetReached_(const cia402::Statusword statusword) +{ + return cia402::targetReached(statusword); +} + +std::int32_t Cia402Protocol::radToCounts_(const double angle_rad, + const NodeState& state) const +{ + const double rev = angle_rad / (2.0 * M_PI); + return static_cast( + std::llround(rev * state.gear_ratio * state.encoder_counts_per_rev)); +} + +double Cia402Protocol::countsToRad_(const std::int32_t counts, + const NodeState& state) const +{ + return static_cast(counts) / + (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI; +} + +std::int32_t Cia402Protocol::radPerSecToCounts_( + const double velocity_rad_s, + const NodeState& state) const +{ + const double rev_per_sec = velocity_rad_s / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec * state.gear_ratio * state.encoder_counts_per_rev)); +} + +std::int32_t Cia402Protocol::radPerSec2ToCounts_( + const double acceleration_rad_s2, + const NodeState& state) const +{ + const double rev_per_sec2 = acceleration_rad_s2 / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec2 * state.gear_ratio * state.encoder_counts_per_rev)); +} + +double Cia402Protocol::countsToRadPerSec_( + const std::int32_t velocity_counts_s, + const NodeState& state) const +{ + return static_cast(velocity_counts_s) / + (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI; +} + +Cia402Protocol::NodeState& Cia402Protocol::nodeState_(const std::uint8_t node_id) +{ + return nodes_[node_id]; +} + +const Cia402Protocol::NodeState* Cia402Protocol::findNodeState_( + const std::uint8_t node_id) const +{ + const auto it = nodes_.find(node_id); + if (it == nodes_.end()) { + return nullptr; + } + return &it->second; +} + +bool Cia402Protocol::hasValidConversion_(const std::uint8_t node_id, + const NodeState& state) const +{ + if (state.encoder_counts_per_rev > 0.0 && state.gear_ratio > 0.0) { + return true; + } + CMVR_LOG(ERROR) << "[Cia402Protocol] missing conversion config for node " + << static_cast(node_id) + << ": encoder_counts_per_rev=" << state.encoder_counts_per_rev + << ", gear_ratio=" << state.gear_ratio; + return false; +} + +bool Cia402Protocol::validateNodePdos_(const std::uint8_t node_id) const +{ + if (!bus_runtime_) { + return false; + } + struct RequiredEntry { + std::uint16_t index; + std::uint8_t subindex; + const char* name; + }; + const RequiredEntry required[] = { + {msgs::CIA402_CONTROL_WORD_6040, 0x00, "controlword"}, + {msgs::CIA402_TARGET_POSITION_607A, 0x00, "target position"}, + {msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, "target velocity"}, + {msgs::CIA402_TARGET_TORQUE_6071, 0x00, "target torque"}, + {msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, "profile acceleration"}, + {msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, "profile deceleration"}, + {msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, "profile velocity"}, + {msgs::CIA402_OPERATION_MODE_6060, 0x00, "operation mode"}, + {msgs::CIA402_STATUS_WORD_6041, 0x00, "statusword"}, + {msgs::CIA402_ACTUAL_POSITION_6064, 0x00, "actual position"}, + {msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, "actual velocity"}, + {msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, "actual torque"}, + {msgs::CIA402_MODE_DISPLAY_6061, 0x00, "mode display"}, + {msgs::CIA402_ERROR_CODE_603F, 0x00, "error code"}, + }; + + for (const auto& entry : required) { + if (!bus_runtime_->hasPdoEntry(node_id, entry.index, entry.subindex)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] missing PDO entry for node " + << static_cast(node_id) + << ": " << entry.name + << " 0x" << std::hex << entry.index + << ":" << static_cast(entry.subindex) << std::dec; + return false; + } + } + return true; +} + +bool Cia402Protocol::readStatusword_(const std::uint8_t node_id, + std::uint16_t& statusword) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_STATUS_WORD_6041, 0x00, + statusword); +} + +bool Cia402Protocol::readActualPosition_(const std::uint8_t node_id, + std::int32_t& actual_position) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00, + actual_position); +} + +bool Cia402Protocol::readActualVelocity_(const std::uint8_t node_id, + std::int32_t& actual_velocity) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, + actual_velocity); +} + +bool Cia402Protocol::readModeDisplay_(const std::uint8_t node_id, + std::int8_t& mode_display) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00, + mode_display); +} + +bool Cia402Protocol::writeControlword_(const std::uint8_t node_id, + const cia402::Controlword controlword) +{ + if (!bus_runtime_) { + return false; + } + auto& state = nodeState_(node_id); + state.controlword = controlword; + return bus_runtime_->writePdo(node_id, msgs::CIA402_CONTROL_WORD_6040, + 0x00, controlword.value); +} + +bool Cia402Protocol::writeControlwordAndWait_( + const std::uint8_t node_id, + const cia402::Controlword controlword, + const cia402::DeviceState target_state, + const char* state_name) +{ + if (!writeControlword_(node_id, controlword)) { + return false; + } + return waitStatus_(node_id, target_state, state_name); +} + +bool Cia402Protocol::waitStatus_(const std::uint8_t node_id, + const cia402::DeviceState target_state, + const char* state_name) const +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.state_transition_timeout_ms()); + std::uint16_t last_statusword = 0; + do { + if (readStatusword_(node_id, last_statusword) && + cia402::hasState(cia402::statusword(last_statusword), target_state)) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for " + << state_name << ", node=" << static_cast(node_id) + << ", last_statusword=0x" << std::hex << last_statusword << std::dec; + return false; +} + +bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id, + const char* action_name) const +{ + const auto* state = findNodeState_(node_id); + if (state == nullptr || !hasValidConversion_(node_id, *state)) { + return false; + } + + const auto tolerance_counts = std::max( + 1, std::abs(radPerSecToCounts_(config_.stopped_velocity_tolerance_rad_s(), *state))); + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.velocity_stop_timeout_ms()); + std::int32_t last_velocity = 0; + do { + if (readActualVelocity_(node_id, last_velocity) && + std::abs(last_velocity) <= tolerance_counts) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting velocity near zero " + << "during " << action_name + << ", node=" << static_cast(node_id) + << ", last_velocity=" << last_velocity + << ", tolerance=" << tolerance_counts; + return false; +} + +bool Cia402Protocol::writePositionLimitsToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_q_lb) || !std::isfinite(state.limit_q_ub) || + state.limit_q_ub <= state.limit_q_lb) { + return true; + } + + const auto lower_limit = radToCounts_(state.limit_q_lb, state); + const auto upper_limit = radToCounts_(state.limit_q_ub, state); + + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, lower_limit) && + bus_runtime_->writeSdo(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, upper_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write software position " + << "limits to dictionary, node=" << static_cast(node_id) + << ", lower=" << lower_limit + << ", upper=" << upper_limit; + } + return ok; +} + +bool Cia402Protocol::writeVelocityLimitToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_qd) || state.limit_qd <= 0.0) { + return true; + } + + const auto velocity_limit = + static_cast(std::abs(radPerSecToCounts_(state.limit_qd, state))); + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_MAX_PROFILE_VELOCITY_607F, + 0x00, velocity_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write velocity limit " + << "to dictionary, node=" << static_cast(node_id) + << ", velocity_limit=" << velocity_limit; + } + return ok; +} + +bool Cia402Protocol::writeAccelerationLimitsToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_qdd) || state.limit_qdd <= 0.0) { + return true; + } + + const auto acceleration_limit = + static_cast(std::abs(radPerSec2ToCounts_(state.limit_qdd, state))); + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, acceleration_limit) && + bus_runtime_->writeSdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, acceleration_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write acceleration limits " + << "to dictionary, node=" << static_cast(node_id) + << ", acceleration_limit=" << acceleration_limit; + } + return ok; +} + +void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, + NodeState& state) +{ + if (!bus_runtime_) { + return; + } + + if (state.profile_velocity <= 0 && state.limit_qd > 0.0) { + state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state)); + } + if (state.profile_acceleration <= 0 && state.limit_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + + state.controlword = cia402::enableOperationControlword(); + writeNode_(node_id, state); + + state.controlword = cia402::profilePositionControlword(true); + writeControlword_(node_id, state.controlword); + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.profile_position_trigger_delay_ms())); + + state.controlword = cia402::profilePositionControlword(false); + writeControlword_(node_id, state.controlword); +} + +bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + NodeState& state) +{ + state.target_velocity = 0; + state.target_torque = 0; + + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: { + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual " + << "position before switching mode, node=" + << static_cast(node_id) + << ", mode=" << static_cast(mode); + return false; + } + state.target_position = actual_position; + break; + } + + case msgs::RUN_MODE_PROFILE_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + case msgs::RUN_MODE_HOMING: + case msgs::RUN_MODE_UNSPECIFIED: + default: + break; + } + + return true; +} + +void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + const NodeState& state) const +{ + if (!bus_runtime_) { + return; + } + + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position); + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, + 0x00, state.profile_velocity); + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, state.profile_acceleration); + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration); + } + break; + + case msgs::RUN_MODE_PROFILE_VELOCITY: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity); + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, state.profile_acceleration); + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration); + } + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position); + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity); + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity); + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_TORQUE_6071, + 0x00, state.target_torque); + break; + + default: + break; + } +} + +void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state) +{ + if (!bus_runtime_) { + return; + } + + std::uint16_t statusword = 0; + if (readStatusword_(node_id, statusword)) { + const auto status = cia402::statusword(statusword); + state.controlword = nextControlword_(status); + std::int32_t actual_position = 0; + if (!isOperationEnabled_(status) && + readActualPosition_(node_id, actual_position) && + actual_position != 0) { + state.target_position = actual_position; + } + } + + bus_runtime_->writePdo(node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, + state.controlword.value); + bus_runtime_->writePdo(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, + toCia402Mode_(state.mode)); + + writeTargetsForMode_(node_id, state.mode, state); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp new file mode 100644 index 00000000..73ae01ef --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp @@ -0,0 +1,174 @@ +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" + +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +EyouMotor::EyouMotor(const config::MotorConfigItem& config, + std::shared_ptr cia402_protocol, + std::unique_ptr vendor_adapter) + : cia402_protocol_(std::move(cia402_protocol)), + vendor_adapter_(std::move(vendor_adapter)) +{ + info_.id = config.id(); + info_.joint_name = config.joint_name(); + info_.limit_q_lb = config.limit_q_lb(); + info_.limit_q_ub = config.limit_q_ub(); + info_.limit_qd = config.limit_qd(); + info_.limit_qdd = config.limit_qdd(); + encoder_counts_per_rev_ = config.encoder_counts_per_rev(); + gear_ratio_ = config.gear_ratio(); + node_id_ = static_cast(info_.id); + id_ = info_.joint_name; + protocol_ = cia402_protocol_; +} + +bool EyouMotor::init() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_() || !hasValidConversion_()) { + return false; + } + + if (!cia402_protocol_->initNode(node_id_)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name; + return false; + } + if (!vendor_adapter_->initNode(node_id_)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name; + return false; + } + cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); + + cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); + if (!writeVendorVelocityLimit_()) { + return false; + } + if (info_.limit_qdd > 0.0) { + cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd); + } + if (!writeVendorPositionLimits_()) { + return false; + } + return true; +} + +void EyouMotor::setLimitQ(const double ub, const double lb) +{ + std::scoped_lock lock(mtx_); + info_.limit_q_ub = ub; + info_.limit_q_lb = lb; + if (!hasDependencies_() || !hasValidConversion_()) { + return; + } + writeVendorPositionLimits_(); +} + +void EyouMotor::setLimitQd(const double qd) +{ + std::scoped_lock lock(mtx_); + info_.limit_qd = qd; + if (!hasDependencies_() || !hasValidConversion_()) { + return; + } + cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); + writeVendorVelocityLimit_(); +} + +bool EyouMotor::calibrateZeroQ() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_() || !hasValidConversion_()) { + return false; + } + + std::int32_t zeroed_position = 0; + if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) { + CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name; + return false; + } + if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) { + return false; + } + if (!writeVendorPositionLimits_()) { + return false; + } + return true; +} + +bool EyouMotor::brakeRelease() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_()) { + return false; + } + return vendor_adapter_->brakeRelease(node_id_); +} + +bool EyouMotor::hasDependencies_() const +{ + if (!cia402_protocol_ || !vendor_adapter_) { + CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: " + << info_.joint_name; + return false; + } + if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) { + CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name; + return false; + } + return true; +} + +bool EyouMotor::hasValidConversion_() const +{ + if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) { + return true; + } + CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: " + << info_.joint_name + << ", encoder_counts_per_rev=" << encoder_counts_per_rev_ + << ", gear_ratio=" << gear_ratio_; + return false; +} + +bool EyouMotor::writeVendorPositionLimits_() const +{ + if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) || + info_.limit_q_ub <= info_.limit_q_lb) { + return true; + } + + cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); + return vendor_adapter_->writePositionLimits(node_id_, + radToCounts_(info_.limit_q_lb), + radToCounts_(info_.limit_q_ub)); +} + +bool EyouMotor::writeVendorVelocityLimit_() const +{ + if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) { + return true; + } + + return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd)); +} + +std::int32_t EyouMotor::radToCounts_(const double angle_rad) const +{ + const double rev = angle_rad / (2.0 * M_PI); + return static_cast( + std::llround(rev * gear_ratio_ * encoder_counts_per_rev_)); +} + +std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const +{ + const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_)); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp new file mode 100644 index 00000000..f43fee3e --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp @@ -0,0 +1,221 @@ +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +#include +#include +#include +#include +#include +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "common/base/logging/logger.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h" + +namespace cmvr::device { + +EyouMotorAdapter::EyouMotorAdapter( + std::shared_ptr bus_runtime) + : bus_runtime_(std::move(bus_runtime)) +{ +} + +bool EyouMotorAdapter::initNode(const std::uint8_t node_id) +{ + return bus_runtime_ && bus_runtime_->hasMotor(node_id); +} + +bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id, + const std::int32_t lower_limit, + const std::int32_t upper_limit) +{ + if (!bus_runtime_) { + return false; + } + + const auto write_limits = [&]() { + return bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0) && + bus_runtime_->writeSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, upper_limit) && + bus_runtime_->writeSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, lower_limit) && + bus_runtime_->writeSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0x4C494D54); + }; + const auto readback_matches = [&]() { + std::int32_t actual_lower = 0; + std::int32_t actual_upper = 0; + return bus_runtime_->readSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, actual_lower) && + bus_runtime_->readSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, actual_upper) && + actual_lower == lower_limit && + actual_upper == upper_limit; + }; + + const bool ok = write_limits() && readback_matches(); + if (!ok) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position " + << "limits, node=" << static_cast(node_id) + << ", soft_limit_state=" << 0x4C494D54 + << ", lower=" << lower_limit + << ", upper=" << upper_limit; + } + return ok; +} + +bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id, + const std::uint32_t velocity_limit) +{ + if (!bus_runtime_) { + return false; + } + + std::uint32_t actual_velocity_limit = 0; + const bool ok = + bus_runtime_->writeSdo(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024, + 0x00, velocity_limit) && + bus_runtime_->readSdo(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024, + 0x00, actual_velocity_limit) && + actual_velocity_limit == velocity_limit; + if (!ok) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed " + << "threshold, node=" << static_cast(node_id) + << ", expected=" << velocity_limit + << ", actual=" << actual_velocity_limit; + } + return ok; +} + +bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id, + std::int32_t& zeroed_position) +{ + if (!bus_runtime_) { + return false; + } + + if (!bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position " + << "limit before home offset calibration, node=" + << static_cast(node_id); + return false; + } + + if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, 0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node=" + << static_cast(node_id); + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds{50}); + + std::int32_t actual_position = 0; + if (!bus_runtime_->readSdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, actual_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " + << "after clearing home offset, node=" + << static_cast(node_id); + return false; + } + + if (actual_position == std::numeric_limits::min()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home " + << "offset calibration, node=" << static_cast(node_id) + << ", actual_position=" << actual_position; + return false; + } + + const auto home_offset = static_cast(-actual_position); + if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, home_offset)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node=" + << static_cast(node_id) + << ", home_offset=" << home_offset; + return false; + } + + if (!bus_runtime_->writeSdo( + node_id, eyou::EYOU_STORE_PARAMETERS_1010, + 0x01, + 0x65766173)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node=" + << static_cast(node_id); + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds{50}); + + std::int32_t home_offset_readback = 0; + if (!bus_runtime_->readSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, home_offset_readback)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node=" + << static_cast(node_id); + return false; + } + if (home_offset_readback != home_offset) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node=" + << static_cast(node_id) + << ", expected=" << home_offset + << ", actual=" << home_offset_readback; + return false; + } + + if (!bus_runtime_->readSdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, zeroed_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " + << "after writing home offset, node=" + << static_cast(node_id); + return false; + } + if (std::abs(static_cast(zeroed_position)) > 10000) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, " + << "node=" << static_cast(node_id) + << ", actual_position_before=" << actual_position + << ", home_offset=" << home_offset + << ", home_offset_readback=" << home_offset_readback + << ", actual_position_after=" << zeroed_position + << ", tolerance_counts=" << 10000; + return false; + } + + return true; +} + +bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + if (!bus_runtime_->writeSdo( + node_id, eyou::EYOU_BRAKE_CONTROL_2014, + 0x01, + 1)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node=" + << static_cast(node_id); + return false; + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds{1000}; + do { + std::uint8_t brake_state = 0; + if (bus_runtime_->readSdo( + node_id, eyou::EYOU_BRAKE_CONTROL_2014, + 0x02, brake_state) && + (brake_state == 1 || brake_state == 2)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds{10}); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node=" + << static_cast(node_id); + return false; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp new file mode 100644 index 00000000..cb216272 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -0,0 +1,88 @@ +#include +#include + +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "common/config/config_files.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::device { +namespace { + +constexpr const char* kMotorManagerId = "ethercat_motors"; +constexpr const char* kMotorConfigFile = + "devices/motor/ethercat_motors_two_real_test.pb.txt"; + +class DeviceManagerDestroyGuard { +public: + ~DeviceManagerDestroyGuard() + { + DeviceManager::destroyInstance(); + } +}; + +config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig() +{ + config::DeviceManagerConfig config; + config.set_name("eyou_motor_device_manager_real_test"); + config.set_version("test"); + config.set_init_all_motors_when_no_active_joints(true); + + auto* motor_entry = config.add_devices(); + motor_entry->set_id(kMotorManagerId); + motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); + motor_entry->set_config_file(kMotorConfigFile); + motor_entry->set_enable(true); + + return config; +} + +void printMotorState(const int motor_id, const std::shared_ptr& motor) +{ + ASSERT_NE(motor, nullptr); + std::cout << "motor_id=" << motor_id + << ", joint_name=" << motor->jointName() + << ", q=" << motor->getQ() << " rad" + << ", qd=" << motor->getQd() << " rad/s" + << std::endl; +} + +} // namespace + +TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState) +{ + ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + DeviceManagerDestroyGuard guard; + + auto& device_manager = + DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); + auto motor_manager = device_manager.getDevice(kMotorManagerId); + ASSERT_NE(motor_manager, nullptr); + + // for (int motor_id = 1; motor_id <= 4; ++motor_id) { + // printMotorState(motor_id, motor_manager->getMotor(motor_id)); + // } + + auto motor = motor_manager->getMotor(6); + motor->calibrateZeroQ(); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + motor->commandProfileVelocity(-2,5); + + + for (int i = 1; i <= 50; ++i) + { + auto q = motor->getQ(); + auto qd = motor->getQd(); + std::cout << "q=" << q << ", qd=" << qd << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + motor->quickStop(); + +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp new file mode 100644 index 00000000..f74e3158 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp @@ -0,0 +1,548 @@ +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +namespace cmvr::device { +namespace { + +constexpr int kMotorId = 1; +constexpr std::chrono::milliseconds kModeSettleDelay{100}; +constexpr std::chrono::milliseconds kCommandSamplePeriod{100}; +constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; +constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000}; +constexpr double kDefaultGearRatio = 101.0; +constexpr double kEncoderCountsPerMotorRev = 65536.0; +constexpr double kPi = 3.14159265358979323846; + +config::MotorGroupConfig createSingleSlaveGroup() +{ + config::MotorGroupConfig group; + group.set_id("eyou_motor_real_test"); + group.set_bus_type(config::MOTOR_BUS_ETHERCAT); + group.set_vendor(config::MOTOR_VENDOR_EYOU); + group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402); + + auto* ethercat = group.mutable_ethercat(); + ethercat->set_master_index(0); + ethercat->set_cycle_us(1000); + + auto* cia402 = ethercat->mutable_cia402(); + cia402->set_profile_position_trigger_delay_ms(2); + cia402->set_state_transition_timeout_ms(1200); + cia402->set_velocity_stop_timeout_ms(2000); + cia402->set_status_poll_period_ms(10); + cia402->set_stopped_velocity_tolerance_rad_s(0.001); + + auto* slave = ethercat->add_slaves(); + slave->set_motor_id(kMotorId); + slave->set_alias(0); + slave->set_position(0); + + return group; +} + +class RuntimeStopGuard { +public: + explicit RuntimeStopGuard(std::shared_ptr runtime) + : runtime_(std::move(runtime)) + { + } + + ~RuntimeStopGuard() + { + if (runtime_) { + runtime_->stop(); + } + } + +private: + std::shared_ptr runtime_; +}; + +std::shared_ptr startRuntime() +{ + auto runtime = std::make_shared(); + runtime->setPdoMapping(createEyouCia402PdoMapping()); + if (!runtime->init(createSingleSlaveGroup())) { + return nullptr; + } + if (!runtime->start()) { + runtime->stop(); + return nullptr; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + return runtime; +} + +std::shared_ptr createProtocol( + const std::shared_ptr& runtime) +{ + return std::make_shared(runtime, runtime->config().cia402()); +} + +config::MotorConfigItem createMotorConfig() +{ + config::MotorConfigItem config; + config.set_id(kMotorId); + config.set_joint_name("ethercat_test_joint"); + config.set_limit_q_lb(-6.14); + config.set_limit_q_ub(6.14); + config.set_limit_qd(10.0); + config.set_limit_qdd(10.0); + config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev); + config.set_gear_ratio(kDefaultGearRatio); + return config; +} + +std::unique_ptr createMotor( + const std::shared_ptr& runtime) +{ + auto motor = std::make_unique( + createMotorConfig(), + createProtocol(runtime), + std::make_unique(runtime)); + if (!motor->init()) { + return nullptr; + } + return motor; +} + +void printMotorState(const char* label, AbstractMotor& motor) +{ + std::cout << label + << ": motor_q=" << motor.getQ() << " rad" + << ", motor_qd=" << motor.getQd() << " rad/s" + << std::endl; +} + +std::string hex16(const std::uint16_t value) +{ + std::ostringstream oss; + oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0') + << value; + return oss.str(); +} + +void printRawEthercatFeedback(const char* label, + const std::shared_ptr& runtime) +{ + std::uint16_t statusword = 0; + std::int8_t mode_display = 0; + std::int32_t actual_position = 0; + std::int32_t actual_velocity = 0; + std::int16_t actual_torque = 0; + std::uint16_t error_code = 0; + + runtime->readPdo(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword); + runtime->readPdo(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00, + actual_position); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, + actual_velocity); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, + actual_torque); + runtime->readPdo(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code); + + std::cout << label + << ": statusword=" << hex16(statusword) + << ", mode_display=" << static_cast(mode_display) + << ", actual_position=" << actual_position + << ", actual_velocity=" << actual_velocity + << ", actual_torque=" << actual_torque + << ", error_code=" << hex16(error_code) + << std::endl; +} + +void sampleMotorState(AbstractMotor& motor, + const std::chrono::milliseconds duration) +{ + for (auto elapsed = std::chrono::milliseconds{0}; + elapsed < duration; + elapsed += kCommandSamplePeriod) { + std::this_thread::sleep_for(kCommandSamplePeriod); + std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms"; + printMotorState("", motor); + } +} + +double nearbySafeTarget(const double current_q, const double delta_rad) +{ + return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad)); +} + +} // namespace + +TEST(EyouMotorRealTest, ReadMotorStateOnly) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + printMotorState("motor state", *motor); + printRawEthercatFeedback("raw feedback", runtime); + for (int i = 1; i <= 10; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + printMotorState("motor state", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } +} + +TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + printMotorState("before calibrateZeroQ", *motor); + ASSERT_TRUE(motor->calibrateZeroQ()); + printMotorState("after calibrateZeroQ", *motor); + ASSERT_TRUE(motor->torqueOn()); +} + +TEST(EyouMotorRealTest, CommandProfilePosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); +} + +TEST(EyouMotorRealTest, CommandProfileVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); + + std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0)); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); +} + +TEST(EyouMotorRealTest, CommandCyclicPosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds trajectory_duration{15000}; + const double period_s = 6.0; + const double amplitude_rad = 3; + const double phase_rad = 0.0; + const double center_q = motor->getQ(); + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q + << " rad, period=" << period_s + << " s, amplitude=" << amplitude_rad + << " rad, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = trajectory_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_q = center_q + amplitude_rad * std::sin(theta); + const double target_qd = amplitude_rad * omega * std::cos(theta); + + ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_q=" << target_q + << " rad, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } +} + +TEST(EyouMotorRealTest, CommandCyclicVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds trajectory_duration{15000}; + const double period_s = 6.0; + const double velocity_amplitude_rad_s = 5.0; + const double phase_rad = 0.0; + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s + << " s, velocity_amplitude=" << velocity_amplitude_rad_s + << " rad/s, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = trajectory_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_qd = velocity_amplitude_rad_s * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicVelocity(target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl; + ASSERT_TRUE(motor->commandCyclicVelocity(0.0)); + sampleMotorState(*motor, std::chrono::milliseconds{500}); +} + +TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds run_duration{2000}; + const double period_s = 6.0; + const double velocity_amplitude_rad_s = 4.5; + const double phase_rad = 0.0; + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicVelocity(sin), then quickStop at " + << run_duration.count() + << " ms, period=" << period_s + << " s, velocity_amplitude=" << velocity_amplitude_rad_s + << " rad/s, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = run_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_qd = velocity_amplitude_rad_s * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicVelocity(target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInProfilePosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds quick_stop_time{1000}; + const double start_q = motor->getQ(); + const double target_q = nearbySafeTarget(start_q, 4.0); + const double max_qd = 2.0; + const double max_qdd = 10.0; + + std::cout << "motor.commandProfilePosition(" << target_q + << " rad, " << max_qd + << " rad/s, " << max_qdd + << " rad/s^2), then quickStop at " + << quick_stop_time.count() << " ms" << std::endl; + ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd)); + std::cout << "wait " << quick_stop_time.count() + << " ms before quickStop" << std::endl; + sampleMotorState(*motor, quick_stop_time); + printRawEthercatFeedback("raw feedback before quickStop", runtime); + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInProfileVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds quick_stop_time{2000}; + const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0; + const double max_qdd = 10.0; + + std::cout << "motor.commandProfileVelocity(" << target_qd + << " rad/s, " << max_qdd + << " rad/s^2), then quickStop at " + << quick_stop_time.count() << " ms" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd)); + std::cout << "wait " << quick_stop_time.count() + << " ms before quickStop" << std::endl; + sampleMotorState(*motor, quick_stop_time); + printRawEthercatFeedback("raw feedback before quickStop", runtime); + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInCyclicPosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds run_duration{2000}; + const double start_q = motor->getQ(); + const double target_qd = start_q > 0.0 ? -2.0 : 2.0; + + std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q + << " rad, target_qd=" << target_qd + << " rad/s, then quickStop at " + << run_duration.count() + << " ms, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = run_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double target_q = start_q + target_qd * t_s; + + ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_q=" << target_q + << " rad, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +} // namespace cmvr::device