SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add EYOU motor actual current PDO

Browse Source
This commit is contained in:
linbo 2026-08-03 11:17:16 +08:00
parent dcb95e3ccb
commit a35cbb08ff
11 changed files with 197 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cmvr-es/devices/motor/abstract_motor.h
View File

@ -7,10 +7,12 @@
#pragma once
#include <cstdint>
#include <mutex>
#include "devices/abstract_device.h"
#include "common/base/logging/logger.h"
#include "motor/motor_protocol_interface.h"
#include <mutex>
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<MotorProtocolInterface> protocol) {

7
cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
View File

@ -3,6 +3,7 @@
#include <atomic>
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <mutex>
@ -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;

62
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp
View File

@ -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<int>(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<int>(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<int>(subindex);
return false;
}
std::vector<std::uint8_t> 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<std::uint16_t>(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<int>(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<const char*>(data.data()),
static_cast<std::size_t>(string_end - data.begin()));
return true;
}
std::uint32_t EthercatMotorBusRuntime::pdoEntryKey_(const std::uint16_t index,
const std::uint8_t subindex)
{

6
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp
View File

@ -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<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
std::int16_t actual_current = 0;
EXPECT_TRUE(runtime.readPdo<std::int16_t>(
1, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, actual_current));
std::cout << "CIA402 statusword: 0x" << std::hex << statusword
<< ", mode display: " << std::dec << static_cast<int>(mode_display)
<< ", actual current raw: " << actual_current
<< std::endl;
const int hold_ms = 10000;

1
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h vendored
View File

@ -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"),

3
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h vendored
View File

@ -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<EyouMotorAdapter> 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

6
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h vendored
View File

@ -3,6 +3,7 @@
#include <cstdint>
#include <memory>
#include <string>
#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<EthercatMotorBusRuntime> bus_runtime_;

3
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h vendored
View File

@ -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;

75
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp vendored
View File

@ -1,13 +1,33 @@
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <map>
#include <string>
#include <utility>
#include "common/base/logging/logger.h"
namespace cmvr::device {
namespace {
struct MotorCurrentInfo {
double adc_amperes_per_count;
double rated_current_amperes;
};
const std::map<std::string, MotorCurrentInfo> 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<Cia402Protocol> cia402_protocol,
std::unique_ptr<EyouMotorAdapter> 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<double>(raw_current) *
current_scale_ma_per_count_;
const double clamped_current_ma = std::clamp(
current_ma,
static_cast<double>(std::numeric_limits<std::int16_t>::min()),
static_cast<double>(std::numeric_limits<std::int16_t>::max()));
return static_cast<std::int16_t>(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);

21
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp vendored
View File

@ -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<std::int16_t>(
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

5
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp vendored
View File

@ -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<std::uint16_t>(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword);
@ -169,6 +171,8 @@ void printRawEthercatFeedback(const char* label,
actual_velocity);
runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00,
actual_torque);
runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00,
actual_current);
runtime->readPdo<std::uint16_t>(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;
}