feat: add EtherCAT DC monitoring and four-motor sync test
This commit is contained in:
parent
d84f84b5ee
commit
2cda7be4d4
@ -10,6 +10,8 @@ motor {
|
||||
ethercat {
|
||||
master_index: 0
|
||||
cycle_us: 1000
|
||||
slave_op_timeout_ms: 12000
|
||||
slave_state_poll_period_ms: 10
|
||||
|
||||
cia402 {
|
||||
profile_position_trigger_delay_ms: 2
|
||||
@ -19,6 +21,16 @@ motor {
|
||||
stopped_velocity_tolerance_rad_s: 0.001
|
||||
}
|
||||
|
||||
dc {
|
||||
enable: true
|
||||
reference_motor_id: 1
|
||||
sync0_cycle_us: 1000
|
||||
sync0_shift_us: 0
|
||||
sync_reference_clock_period: 1
|
||||
assign_activate: 768
|
||||
sync_monitor_period_ms: 1000
|
||||
}
|
||||
|
||||
slaves { motor_id: 1 alias: 0 position: 0 }
|
||||
slaves { motor_id: 2 alias: 0 position: 1 }
|
||||
slaves { motor_id: 3 alias: 0 position: 2 }
|
||||
|
||||
@ -10,6 +10,8 @@ motor {
|
||||
ethercat {
|
||||
master_index: 0
|
||||
cycle_us: 1000
|
||||
slave_op_timeout_ms: 15000
|
||||
slave_state_poll_period_ms: 10
|
||||
|
||||
cia402 {
|
||||
profile_position_trigger_delay_ms: 2
|
||||
@ -19,6 +21,16 @@ motor {
|
||||
stopped_velocity_tolerance_rad_s: 0.001
|
||||
}
|
||||
|
||||
dc {
|
||||
enable: false
|
||||
reference_motor_id: 1
|
||||
sync0_cycle_us: 1000
|
||||
sync0_shift_us: 0
|
||||
sync_reference_clock_period: 1
|
||||
assign_activate: 768
|
||||
sync_monitor_period_ms: 1000
|
||||
}
|
||||
|
||||
slaves { motor_id: 1 alias: 0 position: 0 }
|
||||
slaves { motor_id: 2 alias: 0 position: 1 }
|
||||
slaves { motor_id: 3 alias: 0 position: 2 }
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
#define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <mutex>
|
||||
@ -87,6 +88,8 @@ private:
|
||||
};
|
||||
|
||||
bool configureSlave_(SlaveRuntime& slave);
|
||||
bool configureDc_();
|
||||
bool waitSlavesOperational_();
|
||||
void cyclicLoop_();
|
||||
void readFeedbackLocked_();
|
||||
void writeCommandsLocked_();
|
||||
@ -139,6 +142,10 @@ private:
|
||||
const PdoEntryRuntime& entry);
|
||||
static void writeEntryValue_(std::uint8_t* domain_data,
|
||||
const PdoEntryRuntime& entry);
|
||||
static std::uint64_t steadyTimeNs_();
|
||||
static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point);
|
||||
static std::uint32_t usToNs_(std::uint32_t value_us);
|
||||
static std::int32_t usToNs_(std::int32_t value_us);
|
||||
|
||||
std::string id_;
|
||||
config::EtherCATConfig config_;
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include <cstddef>
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
@ -35,6 +36,25 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_;
|
||||
return false;
|
||||
}
|
||||
if (!config_.has_slave_op_timeout_ms()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_op_timeout_ms: " << id_;
|
||||
return false;
|
||||
}
|
||||
if (config_.slave_op_timeout_ms() == 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_op_timeout_ms must be positive: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!config_.has_slave_state_poll_period_ms()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_state_poll_period_ms: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (config_.slave_state_poll_period_ms() == 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_state_poll_period_ms must be positive: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!hasValidPdoMapping_()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] PDO mapping is not configured: "
|
||||
<< id_;
|
||||
@ -84,6 +104,11 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
|
||||
}
|
||||
}
|
||||
|
||||
if (!configureDc_()) {
|
||||
releaseMaster_();
|
||||
return false;
|
||||
}
|
||||
|
||||
initialized_ = true;
|
||||
return true;
|
||||
}
|
||||
@ -122,6 +147,12 @@ bool EthercatMotorBusRuntime::start()
|
||||
running_.store(true);
|
||||
cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this);
|
||||
started_ = true;
|
||||
|
||||
if (!waitSlavesOperational_()) {
|
||||
stop();
|
||||
return false;
|
||||
}
|
||||
|
||||
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_;
|
||||
return true;
|
||||
}
|
||||
@ -304,24 +335,249 @@ bool EthercatMotorBusRuntime::configureSlave_(SlaveRuntime& slave)
|
||||
return true;
|
||||
}
|
||||
|
||||
bool EthercatMotorBusRuntime::configureDc_()
|
||||
{
|
||||
if (!config_.has_dc()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing explicit DC config: "
|
||||
<< id_ << ". Add dc { enable: false } or a complete enabled DC config.";
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto& dc = config_.dc();
|
||||
if (!dc.has_enable()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC enable: " << id_;
|
||||
return false;
|
||||
}
|
||||
if (!dc.enable()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!dc.has_reference_motor_id()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC reference_motor_id: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!dc.has_sync0_cycle_us()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_cycle_us: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!dc.has_sync0_shift_us()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_shift_us: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!dc.has_sync_reference_clock_period()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_reference_clock_period: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!dc.has_assign_activate()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC assign_activate: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (!dc.has_sync_monitor_period_ms()) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_monitor_period_ms: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (dc.reference_motor_id() <= 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference_motor_id must be positive: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (dc.sync0_cycle_us() == 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync0_cycle_us must be positive: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (dc.sync_reference_clock_period() == 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_reference_clock_period must be positive: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
if (dc.assign_activate() == 0 || dc.assign_activate() > 0xFFFFU) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC assign_activate must be in [1, 0xFFFF]: "
|
||||
<< id_ << ", value=" << hexIndex_(dc.assign_activate());
|
||||
return false;
|
||||
}
|
||||
if (dc.sync_monitor_period_ms() == 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_monitor_period_ms must be positive: "
|
||||
<< id_;
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto reference_it = slaves_by_motor_id_.find(dc.reference_motor_id());
|
||||
if (reference_it == slaves_by_motor_id_.end() || !reference_it->second.slave_config) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference motor is not configured: "
|
||||
<< id_ << ", reference_motor_id=" << dc.reference_motor_id();
|
||||
return false;
|
||||
}
|
||||
|
||||
const int select_result = ecrt_master_select_reference_clock(
|
||||
master_, reference_it->second.slave_config);
|
||||
if (select_result != 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to select DC reference clock: "
|
||||
<< id_ << ", reference_motor_id=" << dc.reference_motor_id()
|
||||
<< ", result=" << select_result;
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto assign_activate = static_cast<std::uint16_t>(dc.assign_activate());
|
||||
const auto sync0_cycle_ns = usToNs_(dc.sync0_cycle_us());
|
||||
const auto sync0_shift_ns = usToNs_(dc.sync0_shift_us());
|
||||
for (auto& [motor_id, slave] : slaves_by_motor_id_) {
|
||||
const int result = ecrt_slave_config_dc(slave.slave_config,
|
||||
assign_activate,
|
||||
sync0_cycle_ns,
|
||||
sync0_shift_ns,
|
||||
0,
|
||||
0);
|
||||
if (result != 0) {
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure DC: "
|
||||
<< id_ << ", motor_id=" << motor_id
|
||||
<< ", position=" << slave.cfg.position()
|
||||
<< ", result=" << result;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] configured DC: "
|
||||
<< id_
|
||||
<< ", reference_motor_id=" << dc.reference_motor_id()
|
||||
<< ", sync0_cycle_ns=" << sync0_cycle_ns
|
||||
<< ", sync0_shift_ns=" << sync0_shift_ns
|
||||
<< ", sync_reference_clock_period=" << dc.sync_reference_clock_period()
|
||||
<< ", assign_activate=" << hexIndex_(assign_activate)
|
||||
<< ", sync_monitor_period_ms=" << dc.sync_monitor_period_ms();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool EthercatMotorBusRuntime::waitSlavesOperational_()
|
||||
{
|
||||
const auto deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::milliseconds(config_.slave_op_timeout_ms());
|
||||
const auto poll_period =
|
||||
std::chrono::milliseconds(config_.slave_state_poll_period_ms());
|
||||
|
||||
ec_domain_state_t last_domain_state{};
|
||||
int last_domain_result = 0;
|
||||
do {
|
||||
bool all_slaves_operational = true;
|
||||
for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
|
||||
(void)motor_id;
|
||||
ec_slave_config_state_t state{};
|
||||
const int result = ecrt_slave_config_state(slave.slave_config, &state);
|
||||
if (result != 0 ||
|
||||
!state.online ||
|
||||
!state.operational ||
|
||||
state.al_state != EC_AL_STATE_OP) {
|
||||
all_slaves_operational = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
last_domain_result = ecrt_domain_state(domain_, &last_domain_state);
|
||||
const bool domain_complete =
|
||||
last_domain_result == 0 &&
|
||||
last_domain_state.wc_state == EC_WC_COMPLETE;
|
||||
|
||||
if (all_slaves_operational && domain_complete) {
|
||||
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: "
|
||||
<< id_
|
||||
<< ", working_counter=" << last_domain_state.working_counter;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(poll_period);
|
||||
} while (std::chrono::steady_clock::now() < deadline);
|
||||
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] timeout waiting for EtherCAT slaves OP: "
|
||||
<< id_
|
||||
<< ", timeout_ms=" << config_.slave_op_timeout_ms()
|
||||
<< ", domain_result=" << last_domain_result
|
||||
<< ", domain_wc_state=" << static_cast<int>(last_domain_state.wc_state)
|
||||
<< ", working_counter=" << last_domain_state.working_counter;
|
||||
|
||||
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);
|
||||
CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave state: "
|
||||
<< id_
|
||||
<< ", motor_id=" << motor_id
|
||||
<< ", position=" << slave.cfg.position()
|
||||
<< ", result=" << result
|
||||
<< ", online=" << state.online
|
||||
<< ", operational=" << state.operational
|
||||
<< ", al_state=" << static_cast<int>(state.al_state);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void EthercatMotorBusRuntime::cyclicLoop_()
|
||||
{
|
||||
const auto period = std::chrono::microseconds(config_.cycle_us());
|
||||
auto next_time = std::chrono::steady_clock::now();
|
||||
const bool dc_enabled = config_.dc().enable();
|
||||
const auto dc_sync_period = dc_enabled ? config_.dc().sync_reference_clock_period() : 0U;
|
||||
const auto dc_monitor_period =
|
||||
dc_enabled ? std::chrono::milliseconds(config_.dc().sync_monitor_period_ms())
|
||||
: std::chrono::milliseconds(0);
|
||||
std::uint32_t dc_sync_counter = 0;
|
||||
bool dc_monitor_queued = false;
|
||||
auto cycle_time = std::chrono::steady_clock::now();
|
||||
auto next_dc_monitor_time = cycle_time + dc_monitor_period;
|
||||
while (running_.load()) {
|
||||
next_time += period;
|
||||
if (dc_enabled) {
|
||||
ecrt_master_application_time(master_, timePointNs_(cycle_time));
|
||||
}
|
||||
|
||||
ecrt_master_receive(master_);
|
||||
ecrt_domain_process(domain_);
|
||||
if (dc_enabled && dc_monitor_queued) {
|
||||
const std::uint32_t dc_sync_diff_ns =
|
||||
ecrt_master_sync_monitor_process(master_);
|
||||
if (dc_sync_diff_ns == static_cast<std::uint32_t>(-1)) {
|
||||
CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] DC sync monitor failed: "
|
||||
<< id_;
|
||||
} else {
|
||||
CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] dc_sync_diff_ns="
|
||||
<< dc_sync_diff_ns
|
||||
<< ", group=" << id_;
|
||||
}
|
||||
dc_monitor_queued = false;
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
readFeedbackLocked_();
|
||||
writeCommandsLocked_();
|
||||
}
|
||||
ecrt_domain_queue(domain_);
|
||||
if (dc_enabled) {
|
||||
++dc_sync_counter;
|
||||
if (dc_sync_counter >= dc_sync_period) {
|
||||
dc_sync_counter = 0;
|
||||
ecrt_master_sync_reference_clock(master_);
|
||||
}
|
||||
ecrt_master_sync_slave_clocks(master_);
|
||||
|
||||
if (cycle_time >= next_dc_monitor_time) {
|
||||
const int monitor_result = ecrt_master_sync_monitor_queue(master_);
|
||||
if (monitor_result == 0) {
|
||||
dc_monitor_queued = true;
|
||||
} else {
|
||||
CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] failed to queue DC sync monitor: "
|
||||
<< id_ << ", result=" << monitor_result;
|
||||
}
|
||||
do {
|
||||
next_dc_monitor_time += dc_monitor_period;
|
||||
} while (cycle_time >= next_dc_monitor_time);
|
||||
}
|
||||
}
|
||||
ecrt_master_send(master_);
|
||||
|
||||
std::this_thread::sleep_until(next_time);
|
||||
cycle_time += period;
|
||||
std::this_thread::sleep_until(cycle_time);
|
||||
}
|
||||
}
|
||||
|
||||
@ -672,4 +928,27 @@ void EthercatMotorBusRuntime::writeEntryValue_(std::uint8_t* domain_data,
|
||||
}
|
||||
}
|
||||
|
||||
std::uint64_t EthercatMotorBusRuntime::steadyTimeNs_()
|
||||
{
|
||||
return timePointNs_(std::chrono::steady_clock::now());
|
||||
}
|
||||
|
||||
std::uint64_t EthercatMotorBusRuntime::timePointNs_(
|
||||
const std::chrono::steady_clock::time_point time_point)
|
||||
{
|
||||
const auto time_since_epoch = time_point.time_since_epoch();
|
||||
return static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(time_since_epoch).count());
|
||||
}
|
||||
|
||||
std::uint32_t EthercatMotorBusRuntime::usToNs_(const std::uint32_t value_us)
|
||||
{
|
||||
return value_us * 1000U;
|
||||
}
|
||||
|
||||
std::int32_t EthercatMotorBusRuntime::usToNs_(const std::int32_t value_us)
|
||||
{
|
||||
return value_us * 1000;
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
@ -28,6 +28,17 @@ config::MotorGroupConfig createSingleSlaveGroup()
|
||||
auto* ethercat = group.mutable_ethercat();
|
||||
ethercat->set_master_index(0);
|
||||
ethercat->set_cycle_us(1000);
|
||||
ethercat->set_slave_op_timeout_ms(12000);
|
||||
ethercat->set_slave_state_poll_period_ms(10);
|
||||
|
||||
auto* dc = ethercat->mutable_dc();
|
||||
dc->set_enable(true);
|
||||
dc->set_reference_motor_id(1);
|
||||
dc->set_sync0_cycle_us(1000);
|
||||
dc->set_sync0_shift_us(0);
|
||||
dc->set_sync_reference_clock_period(1);
|
||||
dc->set_assign_activate(768);
|
||||
dc->set_sync_monitor_period_ms(1000);
|
||||
|
||||
auto* slave = ethercat->add_slaves();
|
||||
slave->set_motor_id(1);
|
||||
|
||||
@ -1,5 +1,12 @@
|
||||
#include <array>
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
@ -15,6 +22,16 @@ namespace {
|
||||
constexpr const char* kMotorManagerId = "ethercat_motors";
|
||||
constexpr const char* kMotorConfigFile =
|
||||
"devices/motor/ethercat_motors_two_real_test.pb.txt";
|
||||
constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
|
||||
constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
|
||||
constexpr std::chrono::milliseconds kPrintPeriod{100};
|
||||
constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
|
||||
constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
|
||||
constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kFourMotorAmplitudeRad = 0.2;
|
||||
constexpr double kFourMotorPeriodS = 1.0;
|
||||
constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
|
||||
|
||||
class DeviceManagerDestroyGuard {
|
||||
public:
|
||||
@ -24,6 +41,68 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
struct TrackingErrorStats {
|
||||
std::int64_t sample_count{0};
|
||||
double sum_error{0.0};
|
||||
double sum_error_sq{0.0};
|
||||
double max_abs_error{0.0};
|
||||
double sin_projection{0.0};
|
||||
double cos_projection{0.0};
|
||||
|
||||
void add(const double error, const double theta)
|
||||
{
|
||||
++sample_count;
|
||||
sum_error += error;
|
||||
sum_error_sq += error * error;
|
||||
max_abs_error = std::max(max_abs_error, std::fabs(error));
|
||||
sin_projection += error * std::sin(theta);
|
||||
cos_projection += error * std::cos(theta);
|
||||
}
|
||||
|
||||
double mean() const
|
||||
{
|
||||
return sample_count > 0 ? sum_error / static_cast<double>(sample_count) : 0.0;
|
||||
}
|
||||
|
||||
double rms() const
|
||||
{
|
||||
return sample_count > 0
|
||||
? std::sqrt(sum_error_sq / static_cast<double>(sample_count))
|
||||
: 0.0;
|
||||
}
|
||||
|
||||
double fundamentalAmplitude() const
|
||||
{
|
||||
if (sample_count == 0) {
|
||||
return 0.0;
|
||||
}
|
||||
const double scale = 2.0 / static_cast<double>(sample_count);
|
||||
return scale * std::sqrt(sin_projection * sin_projection +
|
||||
cos_projection * cos_projection);
|
||||
}
|
||||
|
||||
double phaseRad() const
|
||||
{
|
||||
return std::atan2(cos_projection, sin_projection);
|
||||
}
|
||||
};
|
||||
|
||||
double normalizePhaseRad(double phase)
|
||||
{
|
||||
while (phase > kPi) {
|
||||
phase -= 2.0 * kPi;
|
||||
}
|
||||
while (phase < -kPi) {
|
||||
phase += 2.0 * kPi;
|
||||
}
|
||||
return phase;
|
||||
}
|
||||
|
||||
double radToDeg(const double rad)
|
||||
{
|
||||
return rad * 180.0 / kPi;
|
||||
}
|
||||
|
||||
config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
|
||||
{
|
||||
config::DeviceManagerConfig config;
|
||||
@ -66,7 +145,7 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
|
||||
// printMotorState(motor_id, motor_manager->getMotor(motor_id));
|
||||
// }
|
||||
|
||||
auto motor = motor_manager->getMotor(6);
|
||||
auto motor = motor_manager->getMotor(4);
|
||||
motor->calibrateZeroQ();
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
@ -85,4 +164,146 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
|
||||
|
||||
}
|
||||
|
||||
TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
|
||||
{
|
||||
ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
|
||||
DeviceManagerDestroyGuard guard;
|
||||
|
||||
auto& device_manager =
|
||||
DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
|
||||
auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
|
||||
ASSERT_NE(motor_manager, nullptr);
|
||||
|
||||
std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
|
||||
for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
|
||||
const int motor_id = kFourMotorIds[i];
|
||||
motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
|
||||
ASSERT_NE(motors[i], nullptr);
|
||||
printMotorState(motor_id, motors[i]);
|
||||
}
|
||||
|
||||
std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
const int motor_id = kFourMotorIds[i];
|
||||
std::cout << "before calibrateZeroQ: ";
|
||||
printMotorState(motor_id, motors[i]);
|
||||
ASSERT_TRUE(motors[i]->calibrateZeroQ());
|
||||
std::cout << "after calibrateZeroQ: ";
|
||||
printMotorState(motor_id, motors[i]);
|
||||
}
|
||||
|
||||
for (const auto& motor : motors) {
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
}
|
||||
for (const auto& motor : motors) {
|
||||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||||
}
|
||||
|
||||
std::array<double, kFourMotorIds.size()> center_q{};
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
center_q[i] = motors[i]->getQ();
|
||||
}
|
||||
|
||||
const double omega = 2.0 * kPi / kFourMotorPeriodS;
|
||||
std::cout << "command four motors in CSP, duration="
|
||||
<< kFourMotorTrajectoryDuration.count()
|
||||
<< " ms, command_period=" << kCyclicCommandPeriod.count()
|
||||
<< " ms, amplitude=" << kFourMotorAmplitudeRad
|
||||
<< " rad, period=" << kFourMotorPeriodS
|
||||
<< " s" << std::endl;
|
||||
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
|
||||
std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
|
||||
double max_error_spread_rad = 0.0;
|
||||
double sum_error_spread_sq = 0.0;
|
||||
std::int64_t error_spread_sample_count = 0;
|
||||
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
|
||||
const auto elapsed = tick * kCyclicCommandPeriod;
|
||||
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
|
||||
|
||||
std::array<double, kFourMotorIds.size()> target_q{};
|
||||
std::array<double, kFourMotorIds.size()> target_qd{};
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
const double theta = omega * t_s + kFourMotorPhaseRad[i];
|
||||
target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta));
|
||||
target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
|
||||
ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
|
||||
}
|
||||
|
||||
if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
|
||||
double min_error = std::numeric_limits<double>::max();
|
||||
double max_error = std::numeric_limits<double>::lowest();
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
const double theta = omega * t_s + kFourMotorPhaseRad[i];
|
||||
const double error = motors[i]->getQ() - target_q[i];
|
||||
error_stats[i].add(error, theta);
|
||||
min_error = std::min(min_error, error);
|
||||
max_error = std::max(max_error, error);
|
||||
}
|
||||
const double error_spread = max_error - min_error;
|
||||
max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
|
||||
sum_error_spread_sq += error_spread * error_spread;
|
||||
++error_spread_sample_count;
|
||||
}
|
||||
|
||||
// if (elapsed.count() % kPrintPeriod.count() == 0) {
|
||||
// std::cout << "t=" << elapsed.count() << " ms" << std::endl;
|
||||
// for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
// std::cout << " motor_id=" << kFourMotorIds[i]
|
||||
// << ", target_q=" << target_q[i]
|
||||
// << " rad, target_qd=" << target_qd[i]
|
||||
// << " rad/s, q=" << motors[i]->getQ()
|
||||
// << " rad, qd=" << motors[i]->getQd()
|
||||
// << " rad/s" << std::endl;
|
||||
// }
|
||||
// }
|
||||
|
||||
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
|
||||
}
|
||||
|
||||
const auto hold_start_time = std::chrono::steady_clock::now();
|
||||
const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
|
||||
for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
|
||||
}
|
||||
std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod);
|
||||
}
|
||||
|
||||
std::cout << "after four motor CSP trajectory" << std::endl;
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
printMotorState(kFourMotorIds[i], motors[i]);
|
||||
}
|
||||
|
||||
const double reference_phase = error_stats.front().phaseRad();
|
||||
const double rms_error_spread =
|
||||
error_spread_sample_count > 0
|
||||
? std::sqrt(sum_error_spread_sq / static_cast<double>(error_spread_sample_count))
|
||||
: 0.0;
|
||||
std::cout << "four motor CSP tracking error statistics, sample_period="
|
||||
<< kStatsSamplePeriod.count()
|
||||
<< " ms, samples=" << error_stats.front().sample_count
|
||||
<< ", max_error_spread=" << max_error_spread_rad
|
||||
<< " rad, rms_error_spread=" << rms_error_spread
|
||||
<< " rad" << std::endl;
|
||||
for (std::size_t i = 0; i < motors.size(); ++i) {
|
||||
const double phase = error_stats[i].phaseRad();
|
||||
const double relative_phase = normalizePhaseRad(phase - reference_phase);
|
||||
const double relative_phase_ms = relative_phase / omega * 1000.0;
|
||||
std::cout << " motor_id=" << kFourMotorIds[i]
|
||||
<< ", mean_error=" << error_stats[i].mean()
|
||||
<< " rad, rms_error=" << error_stats[i].rms()
|
||||
<< " rad, max_abs_error=" << error_stats[i].max_abs_error
|
||||
<< " rad, error_fundamental_amp="
|
||||
<< error_stats[i].fundamentalAmplitude()
|
||||
<< " rad, error_phase=" << phase
|
||||
<< " rad (" << radToDeg(phase)
|
||||
<< " deg), relative_phase_to_motor1=" << relative_phase
|
||||
<< " rad (" << radToDeg(relative_phase)
|
||||
<< " deg, " << relative_phase_ms
|
||||
<< " ms)" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
@ -42,6 +42,8 @@ config::MotorGroupConfig createSingleSlaveGroup()
|
||||
auto* ethercat = group.mutable_ethercat();
|
||||
ethercat->set_master_index(0);
|
||||
ethercat->set_cycle_us(1000);
|
||||
ethercat->set_slave_op_timeout_ms(12000);
|
||||
ethercat->set_slave_state_poll_period_ms(10);
|
||||
|
||||
auto* cia402 = ethercat->mutable_cia402();
|
||||
cia402->set_profile_position_trigger_delay_ms(2);
|
||||
@ -50,6 +52,15 @@ config::MotorGroupConfig createSingleSlaveGroup()
|
||||
cia402->set_status_poll_period_ms(10);
|
||||
cia402->set_stopped_velocity_tolerance_rad_s(0.001);
|
||||
|
||||
auto* dc = ethercat->mutable_dc();
|
||||
dc->set_enable(true);
|
||||
dc->set_reference_motor_id(kMotorId);
|
||||
dc->set_sync0_cycle_us(1000);
|
||||
dc->set_sync0_shift_us(0);
|
||||
dc->set_sync_reference_clock_period(1);
|
||||
dc->set_assign_activate(768);
|
||||
dc->set_sync_monitor_period_ms(1000);
|
||||
|
||||
auto* slave = ethercat->add_slaves();
|
||||
slave->set_motor_id(kMotorId);
|
||||
slave->set_alias(0);
|
||||
|
||||
@ -32,6 +32,16 @@ message Cia402ProtocolConfig {
|
||||
double stopped_velocity_tolerance_rad_s = 5;
|
||||
}
|
||||
|
||||
message EtherCATDcConfig {
|
||||
optional bool enable = 1;
|
||||
optional int32 reference_motor_id = 2;
|
||||
optional uint32 sync0_cycle_us = 3;
|
||||
optional int32 sync0_shift_us = 4;
|
||||
optional uint32 sync_reference_clock_period = 5;
|
||||
optional uint32 assign_activate = 6;
|
||||
optional uint32 sync_monitor_period_ms = 7;
|
||||
}
|
||||
|
||||
message SocketCanConfig {
|
||||
string dev_id = 1;
|
||||
int32 channel_id = 2;
|
||||
@ -41,6 +51,9 @@ message EtherCATConfig {
|
||||
uint32 master_index = 1;
|
||||
int32 cycle_us = 2;
|
||||
Cia402ProtocolConfig cia402 = 3;
|
||||
EtherCATDcConfig dc = 4;
|
||||
optional uint32 slave_op_timeout_ms = 5;
|
||||
optional uint32 slave_state_poll_period_ms = 6;
|
||||
repeated EthercatSlaveConfig slaves = 10;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user