diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 35eee16d..869551e5 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -7,10 +7,12 @@ #pragma once +#include +#include + #include "devices/abstract_device.h" #include "common/base/logging/logger.h" #include "motor/motor_protocol_interface.h" -#include namespace cmvr::device{ @@ -212,6 +214,11 @@ namespace cmvr::device{ return protocol_->getQd(node_id_); } + // Returns the actual motor current in mA when supported by the driver. + virtual std::int16_t getCurrent() { + return 0; + } + // 使用的通讯协议 virtual void setProtocol(std::shared_ptr protocol) { diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h index 5b2c7607..8a4bbae8 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -73,6 +74,12 @@ public: return true; } + bool readSdoString(int motor_id, + std::uint16_t index, + std::uint8_t subindex, + std::string& value, + std::size_t max_size = 128); + private: struct PdoEntryRuntime { EthercatPdoEntryConfig cfg; diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp index 4b5554c5..c2a9a611 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -862,6 +862,68 @@ bool EthercatMotorBusRuntime::readSdoRaw_(const int motor_id, return true; } +bool EthercatMotorBusRuntime::readSdoString(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + std::string& value, + const std::size_t max_size) +{ + value.clear(); + if (max_size == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid SDO string buffer size: " + << max_size << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", motor_id=" << motor_id; + return false; + } + + const auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO string read: " + << motor_id << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + if (!master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO string read: " + << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + + std::vector data(max_size); + std::size_t result_size = 0; + std::uint32_t abort_code = 0; + const auto& slave = slave_it->second; + const int result = ecrt_master_sdo_upload( + master_, + static_cast(slave.cfg.position()), + index, + subindex, + data.data(), + data.size(), + &result_size, + &abort_code); + if (result != 0 || result_size > data.size()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to read SDO string: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", slave_position=" << slave.cfg.position() + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", max_size=" << max_size + << ", result=" << result + << ", result_size=" << result_size + << ", abort_code=" << hexIndex_(abort_code); + return false; + } + + const auto string_end = std::find(data.begin(), data.begin() + result_size, 0); + value.assign(reinterpret_cast(data.data()), + static_cast(string_end - data.begin())); + return true; +} + std::uint32_t EthercatMotorBusRuntime::pdoEntryKey_(const std::uint16_t index, const std::uint8_t subindex) { diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp index d767f4bd..a0558ce9 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp @@ -65,6 +65,7 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword) EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_STATUS_WORD_6041, 0x00)); EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_OPERATION_MODE_6060, 0x00)); EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00)); const bool started = runtime.start(); EXPECT_TRUE(started); @@ -85,8 +86,13 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword) std::int8_t mode_display = 0; EXPECT_TRUE(runtime.readPdo(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display)); + std::int16_t actual_current = 0; + EXPECT_TRUE(runtime.readPdo( + 1, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, actual_current)); + std::cout << "CIA402 statusword: 0x" << std::hex << statusword << ", mode display: " << std::dec << static_cast(mode_display) + << ", actual current raw: " << actual_current << std::endl; const int hold_ms = 10000; 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 index 0993dd57..782f119f 100644 --- 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 @@ -62,6 +62,7 @@ inline EthercatPdoMapping createEyouCia402PdoMapping() 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_ACTUAL_CURRENT_6078, 0x00, 16, "Actual Current"), 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"), 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 index 10f3ee96..65a5c0e1 100644 --- 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 @@ -23,12 +23,14 @@ public: void setLimitQd(double qd) override; bool calibrateZeroQ() override; bool brakeRelease() override; + std::int16_t getCurrent() override; private: bool hasDependencies_() const; bool hasValidConversion_() const; bool writeVendorPositionLimits_() const; bool writeVendorVelocityLimit_() const; + void configureCurrentConversion_(); std::int32_t radToCounts_(double angle_rad) const; std::uint32_t radPerSecToCounts_(double velocity_rad_s) const; @@ -36,6 +38,7 @@ private: std::unique_ptr vendor_adapter_; double encoder_counts_per_rev_{0.0}; double gear_ratio_{0.0}; + double current_scale_ma_per_count_{1.0}; }; } // namespace cmvr::device 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 index a9d95490..80104122 100644 --- 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 @@ -3,6 +3,7 @@ #include #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" @@ -23,6 +24,11 @@ public: bool calibrateZero(std::uint8_t node_id, std::int32_t& zeroed_position) override; bool brakeRelease(std::uint8_t node_id) override; + bool readActualCurrent(std::uint8_t node_id, + std::int16_t& current_value) const; + bool readMotorIdentity(std::uint8_t node_id, + std::string& motor_model, + std::string& motor_version) const; private: std::shared_ptr bus_runtime_; 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 index cee1369d..bcaaae8b 100644 --- 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 @@ -5,6 +5,9 @@ namespace cmvr::device::eyou { +inline constexpr std::uint16_t EYOU_DEVICE_NAME_1008 = 0x1008; +inline constexpr std::uint16_t EYOU_SOFTWARE_VERSION_100A = 0x100A; + inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003; inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014; 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 index 73ae01ef..192ee849 100644 --- 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 @@ -1,13 +1,33 @@ #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include #include #include +#include +#include +#include #include #include "common/base/logging/logger.h" namespace cmvr::device { +namespace { + +struct MotorCurrentInfo { + double adc_amperes_per_count; + double rated_current_amperes; +}; + +const std::map kMotorCurrentMap = { + {"EuPH11", {0.005371094, 3.8}}, + {"EuPH14", {0.007672991, 3.9}}, + {"EuPH17", {0.007672991, 3.9}}, + {"EuPH20", {0.013427734, 6.9}}, +}; + +} // namespace + EyouMotor::EyouMotor(const config::MotorConfigItem& config, std::shared_ptr cia402_protocol, std::unique_ptr vendor_adapter) @@ -42,6 +62,7 @@ bool EyouMotor::init() CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name; return false; } + configureCurrentConversion_(); cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); @@ -109,6 +130,29 @@ bool EyouMotor::brakeRelease() return vendor_adapter_->brakeRelease(node_id_); } +std::int16_t EyouMotor::getCurrent() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_()) { + return 0; + } + + std::int16_t raw_current = 0; + if (!vendor_adapter_->readActualCurrent(node_id_, raw_current)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to read actual current: " + << info_.joint_name; + return 0; + } + + const double current_ma = static_cast(raw_current) * + current_scale_ma_per_count_; + const double clamped_current_ma = std::clamp( + current_ma, + static_cast(std::numeric_limits::min()), + static_cast(std::numeric_limits::max())); + return static_cast(clamped_current_ma); +} + bool EyouMotor::hasDependencies_() const { if (!cia402_protocol_ || !vendor_adapter_) { @@ -157,6 +201,37 @@ bool EyouMotor::writeVendorVelocityLimit_() const return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd)); } +void EyouMotor::configureCurrentConversion_() +{ + std::string motor_model; + std::string motor_version; + if (!vendor_adapter_->readMotorIdentity(node_id_, motor_model, motor_version)) { + CMVR_LOG(WARNING) << "[EyouMotor] failed to read motor identity; actual current " + << "will use the raw PDO value: " << info_.joint_name; + return; + } + + std::string model_prefix = motor_model; + const auto dash_pos = motor_model.find('-'); + if (dash_pos != std::string::npos) { + model_prefix = motor_model.substr(0, dash_pos); + } else if (motor_model.rfind("EuPH", 0) == 0 && motor_model.size() >= 6) { + model_prefix = motor_model.substr(0, 6); + } + + const auto current_info = kMotorCurrentMap.find(model_prefix); + if (current_info == kMotorCurrentMap.end()) { + CMVR_LOG(WARNING) << "[EyouMotor] unsupported motor model for current conversion: " + << motor_model << ", actual current will use the raw PDO value"; + return; + } + + const bool is_v145 = motor_version.find("V145") != std::string::npos; + current_scale_ma_per_count_ = is_v145 + ? current_info->second.rated_current_amperes + : current_info->second.adc_amperes_per_count * 1000.0; +} + std::int32_t EyouMotor::radToCounts_(const double angle_rad) const { const double rev = angle_rad / (2.0 * M_PI); 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 index f43fee3e..dd168318 100644 --- 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 @@ -218,4 +218,25 @@ bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id) return false; } +bool EyouMotorAdapter::readActualCurrent(const std::uint8_t node_id, + std::int16_t& current_value) const +{ + return bus_runtime_ && + bus_runtime_->readPdo( + node_id, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, current_value); +} + +bool EyouMotorAdapter::readMotorIdentity(const std::uint8_t node_id, + std::string& motor_model, + std::string& motor_version) const +{ + if (!bus_runtime_) { + return false; + } + return bus_runtime_->readSdoString( + node_id, eyou::EYOU_DEVICE_NAME_1008, 0x00, motor_model) && + bus_runtime_->readSdoString( + node_id, eyou::EYOU_SOFTWARE_VERSION_100A, 0x00, motor_version); +} + } // 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 index 3311327e..92172420 100644 --- 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 @@ -140,6 +140,7 @@ void printMotorState(const char* label, AbstractMotor& motor) std::cout << label << ": motor_q=" << motor.getQ() << " rad" << ", motor_qd=" << motor.getQd() << " rad/s" + << ", motor_current=" << motor.getCurrent() << " mA" << std::endl; } @@ -159,6 +160,7 @@ void printRawEthercatFeedback(const char* label, std::int32_t actual_position = 0; std::int32_t actual_velocity = 0; std::int16_t actual_torque = 0; + std::int16_t actual_current = 0; std::uint16_t error_code = 0; runtime->readPdo(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword); @@ -169,6 +171,8 @@ void printRawEthercatFeedback(const char* label, actual_velocity); runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, actual_torque); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, + actual_current); runtime->readPdo(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code); std::cout << label @@ -177,6 +181,7 @@ void printRawEthercatFeedback(const char* label, << ", actual_position=" << actual_position << ", actual_velocity=" << actual_velocity << ", actual_torque=" << actual_torque + << ", actual_current=" << actual_current << ", error_code=" << hex16(error_code) << std::endl; }