diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt index 15c2b0de..c49936e0 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -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 } diff --git a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt index 53c04267..c473c5ce 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt +++ b/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt @@ -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 } diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h index 3c6fb5ab..5b2c7607 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -2,6 +2,7 @@ #define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H #include +#include #include #include #include @@ -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_; diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp index 6f7102b6..4b5554c5 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #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(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(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(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(-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 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::chrono::duration_cast(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 diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp index 0547d5cc..d767f4bd 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp @@ -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); diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp index cb216272..3532cf73 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -1,5 +1,12 @@ +#include +#include +#include +#include +#include #include +#include #include +#include #include @@ -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 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 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(sample_count) : 0.0; + } + + double rms() const + { + return sample_count > 0 + ? std::sqrt(sum_error_sq / static_cast(sample_count)) + : 0.0; + } + + double fundamentalAmplitude() const + { + if (sample_count == 0) { + return 0.0; + } + const double scale = 2.0 / static_cast(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(kMotorManagerId); + ASSERT_NE(motor_manager, nullptr); + + std::array, 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(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 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 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(elapsed.count()) / 1000.0; + + std::array target_q{}; + std::array 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::max(); + double max_error = std::numeric_limits::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(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 diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp index f74e3158..80a1156c 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp @@ -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); diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index 920e6325..c31a4d8a 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -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; }