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

fix(ethercat): synchronize motor commands and status handling

Browse Source
This commit is contained in:
lgv 2026-07-30 14:08:42 +08:00
parent c43bab8d4c
commit 3ac7db50fd
19 changed files with 1238 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

@ -1,5 +1,6 @@
#include "arm/motor_robot_arm/include/motor_robot_arm.h" #include "arm/motor_robot_arm/include/motor_robot_arm.h"
#include <algorithm>
#include <chrono> #include <chrono>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <stdexcept> #include <stdexcept>
@ -309,25 +310,31 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
} }
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to set cyclic position mode for joint: " +
joint_name);
}
} }
motors.push_back(std::move(motor)); motors.push_back(std::move(motor));
} }
const auto t0 = std::chrono::steady_clock::now(); const auto t0 = std::chrono::steady_clock::now();
constexpr double fallback_dt = 0.001; constexpr double fallback_dt = 0.001;
std::vector<double> command_velocity(motors.size(), 0.0);
for (std::size_t k = 1; k < samples.size(); ++k) { for (std::size_t k = 1; k < samples.size(); ++k) {
const auto& sample = samples[k]; const auto& sample = samples[k];
if (sample.position.size() != motors.size()) { if (sample.position.size() != motors.size()) {
return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
} }
for (std::size_t i = 0; i < motors.size(); ++i) { std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; std::copy_n(sample.velocity.begin(),
if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) { std::min(sample.velocity.size(), command_velocity.size()),
command_velocity.begin());
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, sample.position, command_velocity)) {
return Result::failure(ArmErrorCode::CommandFailed, return Result::failure(ArmErrorCode::CommandFailed,
"failed to command cyclic position for joint: " + "failed to submit atomic cyclic position command");
motors[i]->jointName());
}
} }
if (k + 1 < samples.size()) { if (k + 1 < samples.size()) {
const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
@ -358,7 +365,11 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
"motor not found for joint: " + joint_names_[i]); "motor not found for joint: " + joint_names_[i]);
} }
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to set cyclic velocity mode for joint: " +
joint_names_[i]);
}
} }
if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) { if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) {
return Result::failure(ArmErrorCode::CommandFailed, return Result::failure(ArmErrorCode::CommandFailed,
@ -481,6 +492,8 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
} }
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(joint_names_.size());
for (std::size_t i = 0; i < joint_names_.size(); ++i) { for (std::size_t i = 0; i < joint_names_.size(); ++i) {
auto motor = getMotor_(joint_names_[i]); auto motor = getMotor_(joint_names_[i]);
if (!motor) { if (!motor) {
@ -488,14 +501,19 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
"motor not found for joint: " + joint_names_[i]); "motor not found for joint: " + joint_names_[i]);
} }
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
}
if (!motor->commandCyclicPosition(target.position[i], 0.0)) {
return Result::failure(ArmErrorCode::CommandFailed, return Result::failure(ArmErrorCode::CommandFailed,
"failed to command cyclic position for joint: " + "failed to set cyclic position mode for joint: " +
joint_names_[i]); joint_names_[i]);
} }
} }
motors.push_back(std::move(motor));
}
const std::vector<double> velocities(motors.size(), 0.0);
if (!motor_manager_->commandCyclicPositionsAtomic(motors, target.position, velocities)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to submit atomic cyclic position command");
}
return Result::success(); return Result::success();
} }
@ -762,7 +780,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
return false; return false;
} }
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return false;
}
} }
motors.push_back(std::move(motor)); motors.push_back(std::move(motor));
} }
@ -776,11 +796,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
if (position.size() != motors.size() || velocity.size() != motors.size()) { if (position.size() != motors.size() || velocity.size() != motors.size()) {
return false; return false;
} }
for (std::size_t j = 0; j < motors.size(); ++j) { if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) {
return false; return false;
} }
}
next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>( next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(dt_segment)); std::chrono::duration<double>(dt_segment));
std::this_thread::sleep_until(next_deadline); std::this_thread::sleep_until(next_deadline);

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

@ -48,13 +48,13 @@ namespace cmvr::device{
DeviceKind kind() const noexcept override { return DeviceKind::Motor; } DeviceKind kind() const noexcept override { return DeviceKind::Motor; }
virtual void setMode(msgs::RunMode mode) { virtual bool setMode(msgs::RunMode mode) {
std::scoped_lock lock(mtx_); std::scoped_lock lock(mtx_);
if (!protocol_) { if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor"; CMVR_LOG(ERROR) << "Protocol not set for motor";
return; return false;
} }
protocol_->setMode(node_id_, mode); return protocol_->setMode(node_id_, mode);
} }
virtual msgs::RunMode getMode() { virtual msgs::RunMode getMode() {

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

@ -3,6 +3,7 @@
#include <atomic> #include <atomic>
#include <chrono> #include <chrono>
#include <cstddef>
#include <cstdint> #include <cstdint>
#include <cstring> #include <cstring>
#include <mutex> #include <mutex>
@ -23,6 +24,22 @@ namespace cmvr::device {
class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime { class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime {
public: public:
struct PdoWrite {
int motor_id{0};
std::uint16_t index{0};
std::uint8_t subindex{0};
std::uint8_t bit_length{0};
std::uint64_t raw_value{0};
};
struct PdoRead {
int motor_id{0};
std::uint16_t index{0};
std::uint8_t subindex{0};
std::uint8_t bit_length{0};
std::uint64_t raw_value{0};
};
bool init(const config::MotorGroupConfig& group_cfg) override; bool init(const config::MotorGroupConfig& group_cfg) override;
bool start() override; bool start() override;
void stop() override; void stop() override;
@ -44,6 +61,37 @@ public:
toRawValue_(value)); toRawValue_(value));
} }
template <typename T>
static PdoWrite makePdoWrite(int motor_id,
std::uint16_t index,
std::uint8_t subindex,
T value)
{
return PdoWrite{motor_id, index, subindex, valueBitLength_<T>(), toRawValue_(value)};
}
bool writePdosAtomic(const PdoWrite* writes, std::size_t count);
template <typename T>
static PdoRead makePdoRead(int motor_id,
std::uint16_t index,
std::uint8_t subindex)
{
return PdoRead{motor_id, index, subindex, valueBitLength_<T>(), 0};
}
template <typename T>
static T pdoReadValue(const PdoRead& read)
{
return fromRawValue_<T>(read.raw_value);
}
bool readPdosAtomic(PdoRead* reads, std::size_t count) const;
std::uint64_t commandGeneration() const { return command_generation_.load(); }
std::uint64_t sentCommandGeneration() const { return sent_command_generation_.load(); }
bool isHealthy() const { return healthy_.load(); }
template <typename T> template <typename T>
bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const
{ {
@ -87,10 +135,31 @@ private:
std::unordered_map<std::uint32_t, PdoEntryRuntime> pdo_entries; std::unordered_map<std::uint32_t, PdoEntryRuntime> pdo_entries;
}; };
struct BusHealthState {
bool initialized{false};
bool healthy{false};
unsigned int slaves_responding{0};
unsigned int master_al_states{0};
bool link_up{false};
int domain_result{0};
unsigned int working_counter{0};
unsigned int wc_state{0};
};
struct SlaveHealthState {
bool initialized{false};
bool healthy{false};
int result{0};
bool online{false};
bool operational{false};
unsigned int al_state{0};
};
bool configureSlave_(SlaveRuntime& slave); bool configureSlave_(SlaveRuntime& slave);
bool configureDc_(); bool configureDc_();
bool waitSlavesOperational_(); bool waitSlavesOperational_();
void cyclicLoop_(); void cyclicLoop_();
void monitorBusHealth_();
void readFeedbackLocked_(); void readFeedbackLocked_();
void writeCommandsLocked_(); void writeCommandsLocked_();
void releaseMaster_(); void releaseMaster_();
@ -159,6 +228,12 @@ private:
mutable std::mutex data_mutex_; mutable std::mutex data_mutex_;
std::thread cyclic_thread_; std::thread cyclic_thread_;
std::atomic<bool> running_{false}; std::atomic<bool> running_{false};
std::atomic<bool> healthy_{false};
std::atomic<bool> health_monitor_enabled_{false};
std::atomic<std::uint64_t> command_generation_{0};
std::atomic<std::uint64_t> sent_command_generation_{0};
BusHealthState last_bus_health_;
std::unordered_map<int, SlaveHealthState> last_slave_health_;
bool initialized_{false}; bool initialized_{false};
bool started_{false}; bool started_{false};
}; };

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

@ -145,6 +145,10 @@ bool EthercatMotorBusRuntime::start()
} }
running_.store(true); running_.store(true);
healthy_.store(false);
health_monitor_enabled_.store(false);
last_bus_health_ = {};
last_slave_health_.clear();
cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this); cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this);
started_ = true; started_ = true;
@ -152,6 +156,7 @@ bool EthercatMotorBusRuntime::start()
stop(); stop();
return false; return false;
} }
health_monitor_enabled_.store(true);
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_; CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_;
return true; return true;
@ -160,6 +165,8 @@ bool EthercatMotorBusRuntime::start()
void EthercatMotorBusRuntime::stop() void EthercatMotorBusRuntime::stop()
{ {
running_.store(false); running_.store(false);
healthy_.store(false);
health_monitor_enabled_.store(false);
if (cyclic_thread_.joinable()) { if (cyclic_thread_.joinable()) {
cyclic_thread_.join(); cyclic_thread_.join();
} }
@ -484,6 +491,7 @@ bool EthercatMotorBusRuntime::waitSlavesOperational_()
last_domain_state.wc_state == EC_WC_COMPLETE; last_domain_state.wc_state == EC_WC_COMPLETE;
if (all_slaves_operational && domain_complete) { if (all_slaves_operational && domain_complete) {
healthy_.store(true);
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: " CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: "
<< id_ << id_
<< ", working_counter=" << last_domain_state.working_counter; << ", working_counter=" << last_domain_state.working_counter;
@ -527,6 +535,8 @@ void EthercatMotorBusRuntime::cyclicLoop_()
bool dc_monitor_queued = false; bool dc_monitor_queued = false;
auto cycle_time = std::chrono::steady_clock::now(); auto cycle_time = std::chrono::steady_clock::now();
auto next_dc_monitor_time = cycle_time + dc_monitor_period; auto next_dc_monitor_time = cycle_time + dc_monitor_period;
const auto bus_monitor_period = std::chrono::milliseconds(100);
auto next_bus_monitor_time = cycle_time;
while (running_.load()) { while (running_.load()) {
if (dc_enabled) { if (dc_enabled) {
ecrt_master_application_time(master_, timePointNs_(cycle_time)); ecrt_master_application_time(master_, timePointNs_(cycle_time));
@ -551,6 +561,7 @@ void EthercatMotorBusRuntime::cyclicLoop_()
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
readFeedbackLocked_(); readFeedbackLocked_();
writeCommandsLocked_(); writeCommandsLocked_();
sent_command_generation_.store(command_generation_.load());
} }
ecrt_domain_queue(domain_); ecrt_domain_queue(domain_);
if (dc_enabled) { if (dc_enabled) {
@ -576,11 +587,109 @@ void EthercatMotorBusRuntime::cyclicLoop_()
} }
ecrt_master_send(master_); ecrt_master_send(master_);
if (health_monitor_enabled_.load() && cycle_time >= next_bus_monitor_time) {
monitorBusHealth_();
do {
next_bus_monitor_time += bus_monitor_period;
} while (cycle_time >= next_bus_monitor_time);
}
cycle_time += period; cycle_time += period;
std::this_thread::sleep_until(cycle_time); std::this_thread::sleep_until(cycle_time);
} }
} }
void EthercatMotorBusRuntime::monitorBusHealth_()
{
if (!master_ || !domain_) {
healthy_.store(false);
return;
}
ec_master_state_t master_state{};
ecrt_master_state(master_, &master_state);
ec_domain_state_t domain_state{};
const int domain_result = ecrt_domain_state(domain_, &domain_state);
bool all_slaves_healthy = true;
for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
ec_slave_config_state_t state{};
const int result = ecrt_slave_config_state(slave.slave_config, &state);
const bool slave_healthy = result == 0 && state.online && state.operational &&
state.al_state == EC_AL_STATE_OP;
const SlaveHealthState current{
true,
slave_healthy,
result,
state.online != 0,
state.operational != 0,
static_cast<unsigned int>(state.al_state),
};
auto& previous = last_slave_health_[motor_id];
const bool changed = !previous.initialized ||
previous.healthy != current.healthy ||
previous.result != current.result ||
previous.online != current.online ||
previous.operational != current.operational ||
previous.al_state != current.al_state;
if (changed && !current.healthy) {
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave communication unhealthy: "
<< id_
<< ", motor_id=" << motor_id
<< ", result=" << current.result
<< ", online=" << current.online
<< ", operational=" << current.operational
<< ", al_state=" << current.al_state;
} else if (changed && previous.initialized && current.healthy) {
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] slave communication recovered: "
<< id_ << ", motor_id=" << motor_id;
}
previous = current;
all_slaves_healthy = all_slaves_healthy && slave_healthy;
}
const bool domain_healthy = domain_result == 0 &&
domain_state.wc_state == EC_WC_COMPLETE;
const bool master_healthy = master_state.link_up &&
master_state.slaves_responding >= slaves_by_motor_id_.size() &&
(master_state.al_states & EC_AL_STATE_OP) != 0;
const bool bus_healthy = master_healthy && domain_healthy && all_slaves_healthy;
const BusHealthState current{
true,
bus_healthy,
master_state.slaves_responding,
static_cast<unsigned int>(master_state.al_states),
master_state.link_up != 0,
domain_result,
domain_state.working_counter,
static_cast<unsigned int>(domain_state.wc_state),
};
const bool changed = !last_bus_health_.initialized ||
last_bus_health_.healthy != current.healthy ||
last_bus_health_.slaves_responding != current.slaves_responding ||
last_bus_health_.master_al_states != current.master_al_states ||
last_bus_health_.link_up != current.link_up ||
last_bus_health_.domain_result != current.domain_result ||
last_bus_health_.working_counter != current.working_counter ||
last_bus_health_.wc_state != current.wc_state;
if (changed && !current.healthy) {
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] bus communication unhealthy: "
<< id_
<< ", link_up=" << current.link_up
<< ", slaves_responding=" << current.slaves_responding
<< ", master_al_states=" << current.master_al_states
<< ", domain_result=" << current.domain_result
<< ", wc_state=" << current.wc_state
<< ", working_counter=" << current.working_counter;
} else if (changed && last_bus_health_.initialized && current.healthy) {
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] bus communication recovered: "
<< id_
<< ", working_counter=" << current.working_counter;
}
last_bus_health_ = current;
healthy_.store(bus_healthy);
}
void EthercatMotorBusRuntime::readFeedbackLocked_() void EthercatMotorBusRuntime::readFeedbackLocked_()
{ {
if (!domain_data_) { if (!domain_data_) {
@ -642,23 +751,51 @@ bool EthercatMotorBusRuntime::writePdoRaw_(const int motor_id,
const std::uint8_t bit_len, const std::uint8_t bit_len,
const std::uint64_t value) const std::uint64_t value)
{ {
const PdoWrite write{motor_id, index, subindex, bit_len, value};
return writePdosAtomic(&write, 1);
}
bool EthercatMotorBusRuntime::writePdosAtomic(const PdoWrite* writes,
const std::size_t count)
{
if (!writes || count == 0) {
return false;
}
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
auto slave_it = slaves_by_motor_id_.find(motor_id);
for (std::size_t i = 0; i < count; ++i) {
const auto& write = writes[i];
if (!isSupportedBitLength_(write.bit_length)) {
return false;
}
const auto slave_it = slaves_by_motor_id_.find(write.motor_id);
if (slave_it == slaves_by_motor_id_.end()) { if (slave_it == slaves_by_motor_id_.end()) {
return false; return false;
} }
auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex)); const auto entry_it = slave_it->second.pdo_entries.find(
if (entry_it == slave_it->second.pdo_entries.end()) { pdoEntryKey_(write.index, write.subindex));
if (entry_it == slave_it->second.pdo_entries.end() ||
!entry_it->second.rx ||
entry_it->second.cfg.bit_len != write.bit_length) {
return false; return false;
} }
auto& entry = entry_it->second; for (std::size_t previous = 0; previous < i; ++previous) {
if (!entry.rx || entry.cfg.bit_len != bit_len) { if (writes[previous].motor_id == write.motor_id &&
writes[previous].index == write.index &&
writes[previous].subindex == write.subindex) {
return false; return false;
} }
entry.value = maskValue_(value, entry.cfg.bit_len);
if (domain_data_) {
writeEntryValue_(domain_data_, entry);
} }
}
for (std::size_t i = 0; i < count; ++i) {
const auto& write = writes[i];
auto& entry = slaves_by_motor_id_.at(write.motor_id)
.pdo_entries.at(pdoEntryKey_(write.index, write.subindex));
entry.value = maskValue_(write.raw_value, entry.cfg.bit_len);
}
command_generation_.fetch_add(1);
return true; return true;
} }
@ -668,20 +805,46 @@ bool EthercatMotorBusRuntime::readPdoRaw_(const int motor_id,
const std::uint8_t bit_len, const std::uint8_t bit_len,
std::uint64_t& value) const std::uint64_t& value) const
{ {
PdoRead read{motor_id, index, subindex, bit_len, 0};
if (!readPdosAtomic(&read, 1)) {
return false;
}
value = read.raw_value;
return true;
}
bool EthercatMotorBusRuntime::readPdosAtomic(PdoRead* reads,
const std::size_t count) const
{
if (!reads || count == 0) {
return false;
}
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
const auto slave_it = slaves_by_motor_id_.find(motor_id); for (std::size_t i = 0; i < count; ++i) {
const auto& read = reads[i];
if (!isSupportedBitLength_(read.bit_length)) {
return false;
}
const auto slave_it = slaves_by_motor_id_.find(read.motor_id);
if (slave_it == slaves_by_motor_id_.end()) { if (slave_it == slaves_by_motor_id_.end()) {
return false; return false;
} }
const auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex)); const auto entry_it = slave_it->second.pdo_entries.find(
if (entry_it == slave_it->second.pdo_entries.end()) { pdoEntryKey_(read.index, read.subindex));
if (entry_it == slave_it->second.pdo_entries.end() ||
entry_it->second.rx ||
entry_it->second.cfg.bit_len != read.bit_length) {
return false; return false;
} }
const auto& entry = entry_it->second;
if (entry.rx || entry.cfg.bit_len != bit_len) {
return false;
} }
value = maskValue_(entry.value, entry.cfg.bit_len);
for (std::size_t i = 0; i < count; ++i) {
auto& read = reads[i];
const auto& entry = slaves_by_motor_id_.at(read.motor_id)
.pdo_entries.at(pdoEntryKey_(read.index, read.subindex));
read.raw_value = maskValue_(entry.value, entry.cfg.bit_len);
}
return true; return true;
} }

1
cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt
View File

@ -1,5 +1,6 @@
add_library(ethercat_motor_driver SHARED add_library(ethercat_motor_driver SHARED
src/cia402/cia402_protocol.cpp src/cia402/cia402_protocol.cpp
src/cia402/cia402_status_monitor.cpp
src/vendor/eyou/eyou_motor.cpp src/vendor/eyou/eyou_motor.cpp
src/vendor/eyou/eyou_motor_adapter.cpp src/vendor/eyou/eyou_motor_adapter.cpp
) )

5
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h
View File

@ -156,6 +156,11 @@ inline bool targetReached(const Statusword status)
return status.target_reached != 0; return status.target_reached != 0;
} }
inline bool setPointAcknowledged(const Statusword status)
{
return (status.value & (1U << 12U)) != 0;
}
} // namespace cmvr::device::cia402 } // namespace cmvr::device::cia402
#endif // CMVR_ES_CIA402_OBJECTS_H #endif // CMVR_ES_CIA402_OBJECTS_H

43
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h
View File

@ -3,7 +3,9 @@
#include <cstdint> #include <cstdint>
#include <memory> #include <memory>
#include <mutex>
#include <unordered_map> #include <unordered_map>
#include <vector>
#include "cmvr/config/motor_config/motor_config.pb.h" #include "cmvr/config/motor_config/motor_config.pb.h"
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
@ -12,15 +14,29 @@
namespace cmvr::device { namespace cmvr::device {
class Cia402StatusMonitor;
class Cia402Protocol final : public MotorProtocolInterface { class Cia402Protocol final : public MotorProtocolInterface {
public: public:
struct CyclicPositionCommand {
std::uint8_t node_id{0};
double target_q{0.0};
double target_qd{0.0};
};
struct MotorFeedback {
std::uint8_t node_id{0};
double q{0.0};
double qd{0.0};
};
explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime, explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
const config::Cia402ProtocolConfig& config); const config::Cia402ProtocolConfig& config);
~Cia402Protocol() override = default; ~Cia402Protocol() override;
bool initNode(std::uint8_t node_id) override; bool initNode(std::uint8_t node_id) override;
void setMode(std::uint8_t node_id, msgs::RunMode mode) override; bool setMode(std::uint8_t node_id, msgs::RunMode mode) override;
msgs::RunMode getMode(std::uint8_t node_id) 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 setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override;
void setLimitQd(std::uint8_t node_id, double qd) override; void setLimitQd(std::uint8_t node_id, double qd) override;
@ -37,6 +53,9 @@ public:
bool commandCyclicPosition(std::uint8_t node_id, bool commandCyclicPosition(std::uint8_t node_id,
double target_q, double target_q,
double target_qd) override; double target_qd) override;
bool commandCyclicPositionsAtomic(const CyclicPositionCommand* commands,
std::size_t count);
bool readFeedbacksAtomic(MotorFeedback* feedbacks, std::size_t count) const;
bool commandCyclicVelocity(std::uint8_t node_id, bool commandCyclicVelocity(std::uint8_t node_id,
double target_qd) override; double target_qd) override;
bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
@ -97,18 +116,32 @@ private:
bool waitStatus_(std::uint8_t node_id, bool waitStatus_(std::uint8_t node_id,
cia402::DeviceState target_state, cia402::DeviceState target_state,
const char* state_name) const; const char* state_name) const;
bool waitMode_(std::uint8_t node_id, std::int8_t target_mode) const;
bool waitSetPointAcknowledged_(std::uint8_t node_id, bool acknowledged) const;
bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_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 writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
bool writeVelocityLimitToDictionary_(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 writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state); 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; bool writeTargetsForMode_(std::uint8_t node_id,
void writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state); msgs::RunMode mode,
void writeNode_(std::uint8_t node_id, NodeState& state); const NodeState& state) const;
bool appendTargetWritesForMode_(
std::uint8_t node_id,
msgs::RunMode mode,
const NodeState& state,
EthercatMotorBusRuntime::PdoWrite* writes,
std::size_t capacity,
std::size_t& count) const;
bool writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state);
bool writeNode_(std::uint8_t node_id, NodeState& state);
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_; std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
std::unique_ptr<Cia402StatusMonitor> status_monitor_;
config::Cia402ProtocolConfig config_; config::Cia402ProtocolConfig config_;
std::unordered_map<std::uint8_t, NodeState> nodes_; std::unordered_map<std::uint8_t, NodeState> nodes_;
std::mutex cyclic_position_mutex_;
std::vector<EthercatMotorBusRuntime::PdoWrite> cyclic_position_writes_;
}; };
} // namespace cmvr::device } // namespace cmvr::device

78
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef CMVR_ES_CIA402_STATUS_MONITOR_H
#define CMVR_ES_CIA402_STATUS_MONITOR_H
#include <atomic>
#include <chrono>
#include <cstdint>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_map>
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
namespace cmvr::device {
class Cia402StatusMonitor final {
public:
Cia402StatusMonitor(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
std::chrono::milliseconds poll_period);
~Cia402StatusMonitor();
Cia402StatusMonitor(const Cia402StatusMonitor&) = delete;
Cia402StatusMonitor& operator=(const Cia402StatusMonitor&) = delete;
void addNode(std::uint8_t node_id);
void setExpectedOperationEnabled(std::uint8_t node_id, bool expected);
bool isNodeOperational(std::uint8_t node_id) const;
private:
struct StatusSample {
bool read_ok{false};
bool transport_healthy{false};
bool expected_operation_enabled{false};
bool operation_enabled{false};
bool status_problem{false};
bool command_blocked{false};
std::uint16_t statusword{0};
std::uint16_t error_code{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};
};
struct NodeMonitorState {
bool expected_operation_enabled{false};
bool has_last_sample{false};
StatusSample last_sample;
};
void monitorLoop_();
void monitorNode_(std::uint8_t node_id, bool expected_operation_enabled);
bool readStatusSample_(std::uint8_t node_id,
bool expected_operation_enabled,
StatusSample& sample) const;
void reportErrorCodeTransition_(std::uint8_t node_id,
bool had_previous,
const StatusSample& previous,
const StatusSample& current) const;
void reportStatuswordTransition_(std::uint8_t node_id,
bool had_previous,
const StatusSample& previous,
const StatusSample& current) const;
static const char* deviceStateName_(std::uint16_t statusword);
static const char* errorCodeDescription_(std::uint16_t error_code);
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
std::chrono::milliseconds poll_period_;
std::mutex monitor_mutex_;
mutable std::mutex states_mutex_;
std::unordered_map<std::uint8_t, NodeMonitorState> states_;
std::atomic<bool> running_{true};
std::thread monitor_thread_;
};
} // namespace cmvr::device
#endif // CMVR_ES_CIA402_STATUS_MONITOR_H

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

@ -3,6 +3,7 @@
#include <cstdint> #include <cstdint>
#include <memory> #include <memory>
#include <vector>
#include "cmvr/config/motor_config/motor_config.pb.h" #include "cmvr/config/motor_config/motor_config.pb.h"
#include "devices/motor/abstract_motor.h" #include "devices/motor/abstract_motor.h"
@ -24,6 +25,15 @@ public:
bool calibrateZeroQ() override; bool calibrateZeroQ() override;
bool brakeRelease() override; bool brakeRelease() override;
static bool commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities);
static bool readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities);
private: private:
bool hasDependencies_() const; bool hasDependencies_() const;
bool hasValidConversion_() const; bool hasValidConversion_() const;

409
cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp
View File

@ -1,6 +1,7 @@
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
#include <algorithm> #include <algorithm>
#include <array>
#include <chrono> #include <chrono>
#include <cmath> #include <cmath>
#include <thread> #include <thread>
@ -9,6 +10,7 @@
#include "common/base/logging/logger.h" #include "common/base/logging/logger.h"
#include "cmvr/msgs/cia402.pb.h" #include "cmvr/msgs/cia402.pb.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h"
namespace cmvr::device { namespace cmvr::device {
@ -18,8 +20,13 @@ Cia402Protocol::Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runt
config_(config) config_(config)
{ {
comm_proto = CommProto::ETHERCAT; comm_proto = CommProto::ETHERCAT;
cyclic_position_writes_.reserve(512);
status_monitor_ = std::make_unique<Cia402StatusMonitor>(
bus_runtime_, std::chrono::milliseconds(config_.status_poll_period_ms()));
} }
Cia402Protocol::~Cia402Protocol() = default;
bool Cia402Protocol::initNode(const std::uint8_t node_id) bool Cia402Protocol::initNode(const std::uint8_t node_id)
{ {
if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) {
@ -36,7 +43,10 @@ bool Cia402Protocol::initNode(const std::uint8_t node_id)
if (readActualPosition_(node_id, actual_position)) { if (readActualPosition_(node_id, actual_position)) {
state.target_position = actual_position; state.target_position = actual_position;
} }
writeNode_(node_id, state); if (!writeNode_(node_id, state)) {
return false;
}
status_monitor_->addNode(node_id);
return true; return true;
} }
@ -46,7 +56,8 @@ bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id,
const double max_qdd) const double max_qdd)
{ {
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
if (!hasValidConversion_(node_id, state)) { if (!hasValidConversion_(node_id, state) ||
!status_monitor_->isNodeOperational(node_id)) {
return false; return false;
} }
state.target_position = radToCounts_(target_q, state); state.target_position = radToCounts_(target_q, state);
@ -56,8 +67,7 @@ bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id,
state.profile_acceleration = profile_acceleration; state.profile_acceleration = profile_acceleration;
state.profile_deceleration = profile_acceleration; state.profile_deceleration = profile_acceleration;
} }
writeProfilePositionTarget_(node_id, state); return writeProfilePositionTarget_(node_id, state);
return true;
} }
bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id, bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
@ -65,7 +75,8 @@ bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
const double max_qdd) const double max_qdd)
{ {
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
if (!hasValidConversion_(node_id, state)) { if (!hasValidConversion_(node_id, state) ||
!status_monitor_->isNodeOperational(node_id)) {
return false; return false;
} }
state.target_velocity = radPerSecToCounts_(target_qd, state); state.target_velocity = radPerSecToCounts_(target_qd, state);
@ -74,21 +85,109 @@ bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
state.profile_acceleration = profile_acceleration; state.profile_acceleration = profile_acceleration;
state.profile_deceleration = profile_acceleration; state.profile_deceleration = profile_acceleration;
} }
writeNode_(node_id, state); return writeNode_(node_id, state);
return true;
} }
bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id, bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id,
const double target_q, const double target_q,
const double target_qd) const double target_qd)
{ {
auto& state = nodeState_(node_id); const CyclicPositionCommand command{node_id, target_q, target_qd};
if (!hasValidConversion_(node_id, state)) { return commandCyclicPositionsAtomic(&command, 1);
}
bool Cia402Protocol::commandCyclicPositionsAtomic(
const CyclicPositionCommand* commands,
const std::size_t count)
{
if (!bus_runtime_ || !commands || count == 0 || count > 256) {
return false; return false;
} }
state.target_position = radToCounts_(target_q, state);
state.target_velocity = radPerSecToCounts_(target_qd, state); std::lock_guard<std::mutex> lock(cyclic_position_mutex_);
writeNode_(node_id, state); cyclic_position_writes_.clear();
cyclic_position_writes_.reserve(count * 2);
for (std::size_t i = 0; i < count; ++i) {
const auto& command = commands[i];
const auto* state = findNodeState_(command.node_id);
if (!state || !hasValidConversion_(command.node_id, *state) ||
state->mode != msgs::RUN_MODE_CYCLIC_SYNC_POSITION ||
!status_monitor_->isNodeOperational(command.node_id)) {
return false;
}
for (std::size_t previous = 0; previous < i; ++previous) {
if (commands[previous].node_id == command.node_id) {
return false;
}
}
cyclic_position_writes_.push_back(
EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
command.node_id,
msgs::CIA402_TARGET_POSITION_607A,
0x00,
radToCounts_(command.target_q, *state)));
cyclic_position_writes_.push_back(
EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
command.node_id,
msgs::CIA402_TARGET_VELOCITY_60FF,
0x00,
radPerSecToCounts_(command.target_qd, *state)));
}
if (!bus_runtime_->writePdosAtomic(cyclic_position_writes_.data(),
cyclic_position_writes_.size())) {
return false;
}
for (std::size_t i = 0; i < count; ++i) {
auto& state = nodeState_(commands[i].node_id);
state.target_position = radToCounts_(commands[i].target_q, state);
state.target_velocity = radPerSecToCounts_(commands[i].target_qd, state);
}
return true;
}
bool Cia402Protocol::readFeedbacksAtomic(MotorFeedback* feedbacks,
const std::size_t count) const
{
if (!bus_runtime_ || !feedbacks || count == 0 || count > 256) {
return false;
}
std::array<EthercatMotorBusRuntime::PdoRead, 512> reads{};
for (std::size_t i = 0; i < count; ++i) {
const auto* state = findNodeState_(feedbacks[i].node_id);
if (!state || !hasValidConversion_(feedbacks[i].node_id, *state)) {
return false;
}
for (std::size_t previous = 0; previous < i; ++previous) {
if (feedbacks[previous].node_id == feedbacks[i].node_id) {
return false;
}
}
reads[2 * i] = EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
feedbacks[i].node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00);
reads[2 * i + 1] = EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
feedbacks[i].node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00);
}
if (!bus_runtime_->readPdosAtomic(reads.data(), count * 2)) {
return false;
}
for (std::size_t i = 0; i < count; ++i) {
const auto* state = findNodeState_(feedbacks[i].node_id);
if (!state) {
return false;
}
const auto position = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[2 * i]);
const auto velocity =
EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[2 * i + 1]);
feedbacks[i].q = countsToRad_(position, *state);
feedbacks[i].qd = countsToRadPerSec_(velocity, *state);
}
return true; return true;
} }
@ -96,12 +195,12 @@ bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id,
const double target_qd) const double target_qd)
{ {
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
if (!hasValidConversion_(node_id, state)) { if (!hasValidConversion_(node_id, state) ||
!status_monitor_->isNodeOperational(node_id)) {
return false; return false;
} }
state.target_velocity = radPerSecToCounts_(target_qd, state); state.target_velocity = radPerSecToCounts_(target_qd, state);
writeNode_(node_id, state); return writeTargetsForMode_(node_id, state.mode, state);
return true;
} }
bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id, bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id,
@ -113,15 +212,20 @@ bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id,
return false; return false;
} }
void Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode) bool Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode)
{ {
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
if (!prepareSafeTargetsForMode_(node_id, mode, state)) { if (!prepareSafeTargetsForMode_(node_id, mode, state)) {
return; return false;
} }
writeTargetsForMode_(node_id, mode, state);
state.mode = mode; state.mode = mode;
writeNode_(node_id, state); if (!writeNode_(node_id, state)) {
CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write mode/controlword PDOs, node="
<< static_cast<int>(node_id)
<< ", mode=" << static_cast<int>(mode);
return false;
}
return waitMode_(node_id, toCia402Mode_(mode));
} }
msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id) msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id)
@ -201,6 +305,7 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
return false; return false;
} }
status_monitor_->setExpectedOperationEnabled(node_id, false);
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
std::uint16_t statusword = 0; std::uint16_t statusword = 0;
if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) { if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) {
@ -217,7 +322,9 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
state.mode = msgs::RUN_MODE_PROFILE_POSITION; state.mode = msgs::RUN_MODE_PROFILE_POSITION;
state.target_velocity = 0; state.target_velocity = 0;
state.target_torque = 0; state.target_torque = 0;
writeTargetsForMode_(node_id, state.mode, state); if (!writeTargetsForMode_(node_id, state.mode, state)) {
return false;
}
if (!bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, if (!bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00,
toCia402Mode_(state.mode))) { toCia402Mode_(state.mode))) {
return false; return false;
@ -238,22 +345,22 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
"Operation Enabled")) { "Operation Enabled")) {
return false; return false;
} }
if (!waitMode_(node_id, toCia402Mode_(state.mode))) {
return false;
}
std::int32_t actual_position = 0; std::int32_t actual_position = 0;
if (readActualPosition_(node_id, actual_position)) { if (readActualPosition_(node_id, actual_position)) {
state.target_position = 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); state.controlword = cia402::profilePositionControlword(false);
return writeControlword_(node_id, state.controlword); const bool enabled = writeTargetsForMode_(node_id, state.mode, state) &&
writeControlword_(node_id, state.controlword) &&
waitSetPointAcknowledged_(node_id, false);
if (enabled) {
status_monitor_->setExpectedOperationEnabled(node_id, true);
}
return enabled;
} }
bool Cia402Protocol::torqueOff(const std::uint8_t node_id) bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
@ -262,6 +369,7 @@ bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
return false; return false;
} }
status_monitor_->setExpectedOperationEnabled(node_id, false);
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
std::int32_t actual_position = 0; std::int32_t actual_position = 0;
if (readActualPosition_(node_id, actual_position)) { if (readActualPosition_(node_id, actual_position)) {
@ -269,21 +377,41 @@ bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
} }
state.target_velocity = 0; state.target_velocity = 0;
state.target_torque = 0; state.target_torque = 0;
writeTargetsForMode_(node_id, state.mode, state); if (!writeTargetsForMode_(node_id, state.mode, state)) {
return false;
}
waitVelocityNearZero_(node_id, "torqueOff"); waitVelocityNearZero_(node_id, "torqueOff");
std::uint16_t statusword = 0;
if (!readStatusword_(node_id, statusword)) {
return false;
}
const auto status = cia402::statusword(statusword);
if (cia402::hasState(status, cia402::DeviceState::SwitchOnDisabled) ||
cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
return true;
}
if (cia402::hasState(status, cia402::DeviceState::OperationEnabled)) {
if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
cia402::DeviceState::SwitchedOn, cia402::DeviceState::SwitchedOn,
"Switched On")) { "Switched On")) {
return false; return false;
} }
if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), return writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
cia402::DeviceState::ReadyToSwitchOn, cia402::DeviceState::ReadyToSwitchOn,
"Ready To Switch On")) { "Ready To Switch On");
return false;
} }
return true; if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) {
return writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
cia402::DeviceState::ReadyToSwitchOn,
"Ready To Switch On");
}
return writeControlwordAndWait_(node_id, cia402::controlword(0),
cia402::DeviceState::SwitchOnDisabled,
"Switch On Disabled");
} }
bool Cia402Protocol::brakeRelease(const std::uint8_t node_id) bool Cia402Protocol::brakeRelease(const std::uint8_t node_id)
@ -299,10 +427,13 @@ bool Cia402Protocol::quickStop(const std::uint8_t node_id)
return false; return false;
} }
status_monitor_->setExpectedOperationEnabled(node_id, false);
auto& state = nodeState_(node_id); auto& state = nodeState_(node_id);
state.target_velocity = 0; state.target_velocity = 0;
state.target_torque = 0; state.target_torque = 0;
writeTargetsForMode_(node_id, state.mode, state); if (!writeTargetsForMode_(node_id, state.mode, state)) {
return false;
}
state.controlword = cia402::quickStopControlword(); state.controlword = cia402::quickStopControlword();
if (!writeControlword_(node_id, state.controlword)) { if (!writeControlword_(node_id, state.controlword)) {
@ -350,8 +481,7 @@ bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id)
state.target_position = actual_position; state.target_position = actual_position;
state.target_velocity = 0; state.target_velocity = 0;
state.target_torque = 0; state.target_torque = 0;
writeTargetsForMode_(node_id, state.mode, state); return writeTargetsForMode_(node_id, state.mode, state);
return true;
} }
std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode) std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode)
@ -611,6 +741,50 @@ bool Cia402Protocol::waitStatus_(const std::uint8_t node_id,
return false; return false;
} }
bool Cia402Protocol::waitMode_(const std::uint8_t node_id,
const std::int8_t target_mode) const
{
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::milliseconds(config_.state_transition_timeout_ms());
std::int8_t last_mode = 0;
do {
if (readModeDisplay_(node_id, last_mode) && last_mode == target_mode) {
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 operation mode, node="
<< static_cast<int>(node_id)
<< ", target_mode=" << static_cast<int>(target_mode)
<< ", last_mode=" << static_cast<int>(last_mode);
return false;
}
bool Cia402Protocol::waitSetPointAcknowledged_(
const std::uint8_t node_id,
const bool acknowledged) 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::setPointAcknowledged(cia402::statusword(last_statusword)) == acknowledged) {
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 set-point acknowledge="
<< acknowledged
<< ", node=" << static_cast<int>(node_id)
<< ", last_statusword=0x" << std::hex << last_statusword << std::dec;
return false;
}
bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id, bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id,
const char* action_name) const const char* action_name) const
{ {
@ -720,11 +894,11 @@ bool Cia402Protocol::writeAccelerationLimitsToDictionary_(
return ok; return ok;
} }
void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, bool Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
NodeState& state) NodeState& state)
{ {
if (!bus_runtime_) { if (!bus_runtime_) {
return; return false;
} }
if (state.profile_velocity <= 0 && state.limit_qd > 0.0) { if (state.profile_velocity <= 0 && state.limit_qd > 0.0) {
@ -736,16 +910,27 @@ void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
state.profile_deceleration = profile_acceleration; state.profile_deceleration = profile_acceleration;
} }
state.controlword = cia402::enableOperationControlword(); if (!writeTargetsForMode_(node_id, state.mode, state)) {
writeNode_(node_id, state); return false;
}
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);
if (!writeControlword_(node_id, state.controlword) ||
!waitSetPointAcknowledged_(node_id, false)) {
return false;
}
state.controlword = cia402::profilePositionControlword(true);
if (!writeControlword_(node_id, state.controlword) ||
!waitSetPointAcknowledged_(node_id, true)) {
state.controlword = cia402::profilePositionControlword(false); state.controlword = cia402::profilePositionControlword(false);
writeControlword_(node_id, state.controlword); writeControlword_(node_id, state.controlword);
return false;
}
state.controlword = cia402::profilePositionControlword(false);
return writeControlword_(node_id, state.controlword) &&
waitSetPointAcknowledged_(node_id, false);
} }
bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id, bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id,
@ -782,71 +967,132 @@ bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id,
return true; return true;
} }
void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id, bool Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id,
const msgs::RunMode mode, const msgs::RunMode mode,
const NodeState& state) const const NodeState& state) const
{ {
if (!bus_runtime_) { if (!bus_runtime_) {
return; return false;
} }
std::array<EthercatMotorBusRuntime::PdoWrite, 4> writes{};
std::size_t count = 0;
if (!appendTargetWritesForMode_(node_id, mode, state,
writes.data(), writes.size(), count)) {
return false;
}
return count == 0 || bus_runtime_->writePdosAtomic(writes.data(), count);
}
bool Cia402Protocol::appendTargetWritesForMode_(
const std::uint8_t node_id,
const msgs::RunMode mode,
const NodeState& state,
EthercatMotorBusRuntime::PdoWrite* writes,
const std::size_t capacity,
std::size_t& count) const
{
if (!bus_runtime_ || !writes) {
return false;
}
const auto append = [&](const auto write) {
if (count >= capacity) {
return false;
}
writes[count++] = write;
return true;
};
switch (mode) { switch (mode) {
case msgs::RUN_MODE_PROFILE_POSITION: case msgs::RUN_MODE_PROFILE_POSITION:
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_POSITION_607A, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.target_position); node_id, msgs::CIA402_TARGET_POSITION_607A,
0x00, state.target_position))) {
return false;
}
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) { if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) {
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.profile_velocity); node_id, msgs::CIA402_PROFILE_VELOCITY_6081,
0x00, state.profile_velocity))) {
return false;
}
} }
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) {
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.profile_acceleration); node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
0x00, state.profile_acceleration))) {
return false;
}
} }
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) {
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.profile_deceleration); node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
0x00, state.profile_deceleration))) {
return false;
}
} }
break; break;
case msgs::RUN_MODE_PROFILE_VELOCITY: case msgs::RUN_MODE_PROFILE_VELOCITY:
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.target_velocity); node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
0x00, state.target_velocity))) {
return false;
}
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) {
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.profile_acceleration); node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
0x00, state.profile_acceleration))) {
return false;
}
} }
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) {
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.profile_deceleration); node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
0x00, state.profile_deceleration))) {
return false;
}
} }
break; break;
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_POSITION_607A, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.target_position); node_id, msgs::CIA402_TARGET_POSITION_607A,
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, state.target_position)) ||
0x00, state.target_velocity); !append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
0x00, state.target_velocity))) {
return false;
}
break; break;
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
0x00, state.target_velocity); node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
0x00, state.target_velocity))) {
return false;
}
break; break;
case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
bus_runtime_->writePdo<std::int16_t>(node_id, msgs::CIA402_TARGET_TORQUE_6071, if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int16_t>(
0x00, state.target_torque); node_id, msgs::CIA402_TARGET_TORQUE_6071,
0x00, state.target_torque))) {
return false;
}
break; break;
default: default:
break; break;
} }
return true;
} }
void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state) bool Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
{ {
if (!bus_runtime_) { if (!bus_runtime_) {
return; return false;
} }
std::uint16_t statusword = 0; std::uint16_t statusword = 0;
@ -861,12 +1107,17 @@ void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
} }
} }
bus_runtime_->writePdo<std::uint16_t>(node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, std::array<EthercatMotorBusRuntime::PdoWrite, 6> writes{};
state.controlword.value); std::size_t count = 0;
bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, writes[count++] = EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
toCia402Mode_(state.mode)); node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, state.controlword.value);
writes[count++] = EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
writeTargetsForMode_(node_id, state.mode, state); node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, toCia402Mode_(state.mode));
if (!appendTargetWritesForMode_(node_id, state.mode, state,
writes.data(), writes.size(), count)) {
return false;
}
return bus_runtime_->writePdosAtomic(writes.data(), count);
} }
} // namespace cmvr::device } // namespace cmvr::device

284
cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp Normal file
View File

@ -0,0 +1,284 @@
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h"
#include <algorithm>
#include <array>
#include <iomanip>
#include <utility>
#include "cmvr/msgs/cia402.pb.h"
#include "common/base/logging/logger.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
namespace cmvr::device {
Cia402StatusMonitor::Cia402StatusMonitor(
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
const std::chrono::milliseconds poll_period)
: bus_runtime_(std::move(bus_runtime)),
poll_period_(std::max(poll_period, std::chrono::milliseconds{1})),
monitor_thread_(&Cia402StatusMonitor::monitorLoop_, this)
{
}
Cia402StatusMonitor::~Cia402StatusMonitor()
{
running_.store(false);
if (monitor_thread_.joinable()) {
monitor_thread_.join();
}
}
void Cia402StatusMonitor::addNode(const std::uint8_t node_id)
{
{
std::lock_guard<std::mutex> lock(states_mutex_);
states_.try_emplace(node_id);
}
monitorNode_(node_id, false);
}
void Cia402StatusMonitor::setExpectedOperationEnabled(
const std::uint8_t node_id,
const bool expected)
{
{
std::lock_guard<std::mutex> lock(states_mutex_);
states_[node_id].expected_operation_enabled = expected;
}
monitorNode_(node_id, expected);
}
bool Cia402StatusMonitor::isNodeOperational(const std::uint8_t node_id) const
{
if (!bus_runtime_ || !bus_runtime_->isHealthy()) {
return false;
}
std::lock_guard<std::mutex> lock(states_mutex_);
const auto it = states_.find(node_id);
return it != states_.end() && it->second.has_last_sample &&
it->second.last_sample.read_ok &&
it->second.last_sample.transport_healthy &&
it->second.last_sample.operation_enabled &&
!it->second.last_sample.command_blocked;
}
void Cia402StatusMonitor::monitorLoop_()
{
while (running_.load()) {
std::array<std::pair<std::uint8_t, bool>, 256> nodes{};
std::size_t node_count = 0;
{
std::lock_guard<std::mutex> lock(states_mutex_);
for (const auto& [node_id, state] : states_) {
if (node_count >= nodes.size()) {
break;
}
nodes[node_count++] = {node_id, state.expected_operation_enabled};
}
}
for (std::size_t i = 0; i < node_count; ++i) {
monitorNode_(nodes[i].first, nodes[i].second);
}
std::this_thread::sleep_for(poll_period_);
}
}
void Cia402StatusMonitor::monitorNode_(
const std::uint8_t node_id,
const bool expected_operation_enabled)
{
std::lock_guard<std::mutex> monitor_lock(monitor_mutex_);
StatusSample current;
readStatusSample_(node_id, expected_operation_enabled, current);
StatusSample previous;
bool had_previous = false;
{
std::lock_guard<std::mutex> lock(states_mutex_);
auto& state = states_[node_id];
had_previous = state.has_last_sample;
previous = state.last_sample;
state.last_sample = current;
state.has_last_sample = true;
}
if (!current.transport_healthy) {
return;
}
if (!current.read_ok) {
if (!had_previous || previous.read_ok) {
CMVR_LOG(ERROR) << "[Cia402StatusMonitor] failed to read node status snapshot"
<< ", node=" << static_cast<int>(node_id);
}
return;
}
if (had_previous && !previous.read_ok) {
CMVR_LOG(INFO) << "[Cia402StatusMonitor] node status snapshot recovered"
<< ", node=" << static_cast<int>(node_id);
}
reportErrorCodeTransition_(node_id, had_previous, previous, current);
reportStatuswordTransition_(node_id, had_previous, previous, current);
}
void Cia402StatusMonitor::reportErrorCodeTransition_(
const std::uint8_t node_id,
const bool had_previous,
const StatusSample& previous,
const StatusSample& current) const
{
const bool error_code_changed =
!had_previous || !previous.read_ok ||
previous.error_code != current.error_code;
if (current.error_code != 0 && error_code_changed) {
CMVR_LOG(ERROR) << "[Cia402StatusMonitor] [error code] 0x"
<< std::hex << std::uppercase << std::setw(4)
<< std::setfill('0') << current.error_code
<< std::dec << std::nouppercase << std::setfill(' ')
<< ' ' << errorCodeDescription_(current.error_code)
<< ", node=" << static_cast<int>(node_id);
} else if (current.error_code == 0 && had_previous && previous.read_ok &&
previous.error_code != 0) {
CMVR_LOG(INFO) << "[Cia402StatusMonitor] [error code] recovered"
<< ", node=" << static_cast<int>(node_id)
<< ", previous_code=0x" << std::hex << std::uppercase
<< std::setw(4) << std::setfill('0') << previous.error_code
<< std::dec << std::nouppercase << std::setfill(' ');
}
}
void Cia402StatusMonitor::reportStatuswordTransition_(
const std::uint8_t node_id,
const bool had_previous,
const StatusSample& previous,
const StatusSample& current) const
{
const bool status_changed =
!had_previous || !previous.read_ok ||
previous.status_problem != current.status_problem ||
(previous.statusword & 0x0888) != (current.statusword & 0x0888);
if (current.status_problem && status_changed) {
const auto status = cia402::statusword(current.statusword);
CMVR_LOG(WARNING) << "[Cia402StatusMonitor] [statusword] 0x"
<< std::hex << std::uppercase << std::setw(4)
<< std::setfill('0') << current.statusword
<< std::dec << std::nouppercase << std::setfill(' ')
<< ' ' << deviceStateName_(current.statusword)
<< ", node=" << static_cast<int>(node_id)
<< (status.fault != 0 ? ", fault" : "")
<< (status.warning != 0 ? ", warning" : "")
<< (status.internal_limit_active != 0
? ", internal limit active"
: "")
<< (current.expected_operation_enabled &&
!current.operation_enabled
? ", operation not enabled"
: "");
} else if (!current.status_problem && had_previous && previous.read_ok &&
previous.status_problem) {
CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered"
<< ", node=" << static_cast<int>(node_id)
<< ", previous_statusword=0x" << std::hex << std::uppercase
<< std::setw(4) << std::setfill('0') << previous.statusword
<< std::dec << std::nouppercase << std::setfill(' ');
}
}
bool Cia402StatusMonitor::readStatusSample_(
const std::uint8_t node_id,
const bool expected_operation_enabled,
StatusSample& sample) const
{
sample.expected_operation_enabled = expected_operation_enabled;
sample.transport_healthy = bus_runtime_ && bus_runtime_->isHealthy();
if (!sample.transport_healthy) {
return false;
}
std::array reads{
EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
node_id, msgs::CIA402_STATUS_WORD_6041, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
node_id, msgs::CIA402_ERROR_CODE_603F, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int16_t>(
node_id, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00),
};
if (!bus_runtime_->readPdosAtomic(reads.data(), reads.size())) {
return false;
}
sample.statusword = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[0]);
sample.error_code = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[1]);
sample.mode_display = EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(reads[2]);
sample.actual_position = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[3]);
sample.actual_velocity = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[4]);
sample.actual_torque = EthercatMotorBusRuntime::pdoReadValue<std::int16_t>(reads[5]);
sample.read_ok = true;
const auto status = cia402::statusword(sample.statusword);
sample.operation_enabled = cia402::isOperationEnabled(status);
sample.status_problem = status.fault != 0 || status.warning != 0 ||
status.internal_limit_active != 0 ||
(expected_operation_enabled && !sample.operation_enabled);
sample.command_blocked = status.fault != 0 || status.warning != 0 ||
sample.error_code != 0 ||
(expected_operation_enabled && !sample.operation_enabled);
return true;
}
const char* Cia402StatusMonitor::deviceStateName_(const std::uint16_t statusword)
{
if ((statusword & 0x004F) == 0x000F) {
return "FaultReactionActive";
}
if ((statusword & 0x004F) == 0x0008) {
return "Fault";
}
if ((statusword & 0x006F) == 0x0007) {
return "QuickStopActive";
}
const auto status = cia402::statusword(statusword);
if (cia402::isOperationEnabled(status)) {
return "OperationEnabled";
}
if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) {
return "SwitchedOn";
}
if (cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
return "ReadyToSwitchOn";
}
if (cia402::isSwitchOnDisabled(status)) {
return "SwitchOnDisabled";
}
return "NotReadyToSwitchOn";
}
const char* Cia402StatusMonitor::errorCodeDescription_(const std::uint16_t error_code)
{
switch (error_code) {
case 0x0000:
return "no error";
case 0x2310:
return "continuous over-current";
case 0x3210:
return "DC bus over-voltage";
case 0x3220:
return "DC bus under-voltage";
case 0x4210:
return "device over-temperature";
case 0x4310:
return "drive over-temperature";
case 0x8611:
return "position following error";
default:
return "unknown or vendor-specific error";
}
}
} // namespace cmvr::device

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

@ -1,7 +1,11 @@
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
#include <algorithm>
#include <array>
#include <cmath> #include <cmath>
#include <cstdint> #include <cstdint>
#include <functional>
#include <mutex>
#include <utility> #include <utility>
#include "common/base/logging/logger.h" #include "common/base/logging/logger.h"
@ -100,6 +104,102 @@ bool EyouMotor::calibrateZeroQ()
return true; return true;
} }
bool EyouMotor::commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities)
{
if (motors.empty() || motors.size() != positions.size() ||
motors.size() != velocities.size() || motors.size() > 256) {
return false;
}
std::array<EyouMotor*, 256> lock_order{};
std::array<Cia402Protocol::CyclicPositionCommand, 256> commands{};
std::shared_ptr<Cia402Protocol> protocol;
for (std::size_t i = 0; i < motors.size(); ++i) {
const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
return false;
}
if (!protocol) {
protocol = motor->cia402_protocol_;
} else if (protocol.get() != motor->cia402_protocol_.get()) {
CMVR_LOG(ERROR) << "[EyouMotor] batch target motors belong to different "
<< "CiA402 protocols";
return false;
}
for (std::size_t previous = 0; previous < i; ++previous) {
if (lock_order[previous] == motor.get()) {
return false;
}
}
lock_order[i] = motor.get();
commands[i] = Cia402Protocol::CyclicPositionCommand{
motor->node_id_, positions[i], velocities[i]};
}
std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
std::less<EyouMotor*>{});
std::array<std::unique_lock<std::mutex>, 256> locks{};
for (std::size_t i = 0; i < motors.size(); ++i) {
locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
}
return protocol && protocol->commandCyclicPositionsAtomic(commands.data(), motors.size());
}
bool EyouMotor::readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities)
{
if (motors.empty() || motors.size() > 256) {
return false;
}
std::array<EyouMotor*, 256> lock_order{};
std::array<Cia402Protocol::MotorFeedback, 256> feedbacks{};
std::shared_ptr<Cia402Protocol> protocol;
for (std::size_t i = 0; i < motors.size(); ++i) {
const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
return false;
}
if (!protocol) {
protocol = motor->cia402_protocol_;
} else if (protocol.get() != motor->cia402_protocol_.get()) {
CMVR_LOG(ERROR) << "[EyouMotor] batch feedback motors belong to different "
<< "CiA402 protocols";
return false;
}
for (std::size_t previous = 0; previous < i; ++previous) {
if (lock_order[previous] == motor.get()) {
return false;
}
}
lock_order[i] = motor.get();
feedbacks[i].node_id = motor->node_id_;
}
std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
std::less<EyouMotor*>{});
std::array<std::unique_lock<std::mutex>, 256> locks{};
for (std::size_t i = 0; i < motors.size(); ++i) {
locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
}
if (!protocol || !protocol->readFeedbacksAtomic(feedbacks.data(), motors.size())) {
return false;
}
positions.resize(motors.size());
velocities.resize(motors.size());
for (std::size_t i = 0; i < motors.size(); ++i) {
positions[i] = feedbacks[i].q;
velocities[i] = feedbacks[i].qd;
}
return true;
}
bool EyouMotor::brakeRelease() bool EyouMotor::brakeRelease()
{ {
std::scoped_lock lock(mtx_); std::scoped_lock lock(mtx_);

5
cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h
View File

@ -21,7 +21,7 @@ public:
std::string typeName() const override { return "MujocoMotor"; } std::string typeName() const override { return "MujocoMotor"; }
bool init() override; bool init() override;
void setMode(msgs::RunMode mode) override; bool setMode(msgs::RunMode mode) override;
msgs::RunMode getMode() override; msgs::RunMode getMode() override;
bool torqueOn() override; bool torqueOn() override;
bool torqueOff() override; bool torqueOff() override;
@ -45,7 +45,8 @@ public:
double getQ() override; double getQ() override;
double getQd() override; double getQd() override;
static bool setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors, static bool commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions, const std::vector<double>& positions,
const std::vector<double>& velocities); const std::vector<double>& velocities);

18
cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp
View File

@ -42,10 +42,11 @@ bool MujocoMotor::init()
return true; return true;
} }
void MujocoMotor::setMode(const msgs::RunMode mode) bool MujocoMotor::setMode(const msgs::RunMode mode)
{ {
std::scoped_lock lock(mtx_); std::scoped_lock lock(mtx_);
mode_ = mode; mode_ = mode;
return true;
} }
msgs::RunMode MujocoMotor::getMode() msgs::RunMode MujocoMotor::getMode()
@ -216,7 +217,8 @@ double MujocoMotor::getQd()
return qd; return qd;
} }
bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors, bool MujocoMotor::commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions, const std::vector<double>& positions,
const std::vector<double>& velocities) const std::vector<double>& velocities)
{ {
@ -233,7 +235,7 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
clamped_velocities.reserve(motors.size()); clamped_velocities.reserve(motors.size());
for (std::size_t i = 0; i < motors.size(); ++i) { for (std::size_t i = 0; i < motors.size(); ++i) {
const auto& motor = motors[i]; const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
if (!motor) { if (!motor) {
return false; return false;
} }
@ -263,9 +265,13 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
} }
for (std::size_t i = 0; i < motors.size(); ++i) { for (std::size_t i = 0; i < motors.size(); ++i) {
std::scoped_lock lock(motors[i]->mtx_); const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
motors[i]->target_q_ = clamped_positions[i]; if (!motor) {
motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; return false;
}
std::scoped_lock lock(motor->mtx_);
motor->target_q_ = clamped_positions[i];
motor->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
} }
return true; return true;
} }

2
cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h
View File

@ -30,7 +30,7 @@ namespace cmvr {
bool initNode(uint8_t node_id) override; bool initNode(uint8_t node_id) override;
void setMode(uint8_t node_id, msgs::RunMode mode) override; bool setMode(uint8_t node_id, msgs::RunMode mode) override;
void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQ(uint8_t node_id, double ub, double lb) override;
void setLimitQd(uint8_t node_id, double qd) override; void setLimitQd(uint8_t node_id, double qd) override;
void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override; void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;

15
cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp
View File

@ -263,7 +263,7 @@ void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, i
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
} }
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
// cur_mode_[node_id] = mode; // cur_mode_[node_id] = mode;
@ -334,8 +334,16 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
} }
default: default:
// TODO: Handle unspecified or unknown mode // TODO: Handle unspecified or unknown mode
break; return false;
} }
if (!waitUntil([&]() { return getMode(node_id) == mode; }, 500)) {
CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
<< ": operation mode switch failed, target_mode="
<< static_cast<int>(mode);
return false;
}
return true;
} }
void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) { void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
@ -563,8 +571,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
} }
bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) { bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) {
setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); return setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
return true;
} }
bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) { bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) {

8
cmvr-es/devices/motor/manager/include/motor_manager.h
View File

@ -45,6 +45,14 @@ public:
std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const; std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const;
std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const; std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const; const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const;
bool commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities) const;
bool readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities) const;
static std::shared_ptr<MotorManager> managerFor(const std::string& id); static std::shared_ptr<MotorManager> managerFor(const std::string& id);
static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id); static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);

40
cmvr-es/devices/motor/manager/src/motor_manager.cpp
View File

@ -239,6 +239,46 @@ const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& MotorMana
return motors_by_joint_; return motors_by_joint_;
} }
bool MotorManager::commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities) const
{
if (motors.empty() || motors.size() != positions.size() ||
motors.size() != velocities.size()) {
return false;
}
if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
return EyouMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
}
if (std::dynamic_pointer_cast<MujocoMotor>(motors.front())) {
return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
}
CMVR_LOG(ERROR) << "[MotorManager] atomic cyclic position is unsupported for motor type: "
<< motors.front()->typeName();
return false;
}
bool MotorManager::readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities) const
{
if (motors.empty()) {
return false;
}
if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
return EyouMotor::readFeedbacksAtomic(motors, positions, velocities);
}
CMVR_LOG(ERROR) << "[MotorManager] atomic feedback is unsupported for motor type: "
<< motors.front()->typeName();
return false;
}
std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id) std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id)
{ {
std::lock_guard<std::mutex> lock(registry_mutex_); std::lock_guard<std::mutex> lock(registry_mutex_);

2
cmvr-es/devices/motor/motor_protocol_interface.h
View File

@ -27,7 +27,7 @@ namespace cmvr {
*/ */
virtual bool initNode(uint8_t node_id) = 0; virtual bool initNode(uint8_t node_id) = 0;
virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; virtual bool setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual msgs::RunMode getMode(uint8_t node_id) = 0; virtual msgs::RunMode getMode(uint8_t node_id) = 0;
virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
virtual void setLimitQd(uint8_t node_id,double qd) = 0; virtual void setLimitQd(uint8_t node_id,double qd) = 0;