diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index 3bedb230..a9e3f852 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC add_library(cmvr_es::common ALIAS common) install(TARGETS common LIBRARY DESTINATION lib) +add_executable(support_functions_test + math/support_functions_test.cpp +) +target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es) +target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog) + #add_executable(image_display_test # utils/visualization/image_display_test.cpp #) diff --git a/cmvr-es/common/math/support_functions_test.cpp b/cmvr-es/common/math/support_functions_test.cpp new file mode 100644 index 00000000..cf1a7c4e --- /dev/null +++ b/cmvr-es/common/math/support_functions_test.cpp @@ -0,0 +1,34 @@ +#include +#include + +#include + +#include "common/math/support_functions.h" + +TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent) +{ + constexpr std::int64_t period = 65536LL * 101LL; + + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0); + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0); +} + +TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance) +{ + constexpr std::int64_t period = 100; + + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6); + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6); + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30); +} + +TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range) +{ + constexpr std::int64_t period = 65536LL * 101LL; + + const auto distance = SupportFunctions::cyclicAbsoluteDifference( + std::numeric_limits::min(), + std::numeric_limits::max(), period); + EXPECT_GE(distance, 0); + EXPECT_LE(distance, period / 2); +} diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp index d767f4bd..ba470aae 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 @@ -1,6 +1,7 @@ #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include +#include #include #include #include @@ -73,17 +74,42 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword) return; } - EXPECT_TRUE(runtime.writePdo(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000)); - EXPECT_TRUE(runtime.writePdo(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0)); + const std::array command_writes{ + EthercatMotorBusRuntime::makePdoWrite( + 1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000), + EthercatMotorBusRuntime::makePdoWrite( + 1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0), + }; + auto invalid_writes = command_writes; + invalid_writes[1].bit_length = 16; + + const auto generation_before = runtime.commandGeneration(); + EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size())); + EXPECT_EQ(runtime.commandGeneration(), generation_before); + EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size())); + EXPECT_EQ(runtime.commandGeneration(), generation_before + 1); const int settle_ms = 1000; std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms)); + EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1); + EXPECT_TRUE(runtime.isHealthy()); - std::uint16_t statusword = 0; - EXPECT_TRUE(runtime.readPdo(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword)); + std::array feedback_reads{ + EthercatMotorBusRuntime::makePdoRead( + 1, msgs::CIA402_STATUS_WORD_6041, 0x00), + EthercatMotorBusRuntime::makePdoRead( + 1, msgs::CIA402_MODE_DISPLAY_6061, 0x00), + }; + auto invalid_feedback_reads = feedback_reads; + invalid_feedback_reads[1].bit_length = 16; + EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(), + invalid_feedback_reads.size())); + ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size())); - std::int8_t mode_display = 0; - EXPECT_TRUE(runtime.readPdo(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display)); + const auto statusword = + EthercatMotorBusRuntime::pdoReadValue(feedback_reads[0]); + const auto mode_display = + EthercatMotorBusRuntime::pdoReadValue(feedback_reads[1]); std::cout << "CIA402 statusword: 0x" << std::hex << statusword << ", mode display: " << std::dec << static_cast(mode_display) 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 9c2fbe9b..3b877f00 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 @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include @@ -24,14 +26,18 @@ constexpr const char* kMotorConfigFile = "devices/motor/ethercat_motors_four_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 double kRaisedCosineCoefficientRad = 0.2; +constexpr double kFourMotorPeriodS = 2.0; constexpr std::array kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0}; +constexpr double kMinimumPositionExcursionRad = 0.2; +constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15; +constexpr double kMaximumRmsTrackingErrorRad = 0.08; +constexpr double kMaximumErrorSpreadRad = 0.10; +constexpr double kFinalPositionToleranceRad = 0.05; class DeviceManagerDestroyGuard { public: @@ -41,6 +47,61 @@ public: } }; +class MotorManagerStopGuard { +public: + explicit MotorManagerStopGuard(std::shared_ptr motor_manager) + : motor_manager_(std::move(motor_manager)) + { + } + + ~MotorManagerStopGuard() + { + if (motor_manager_ && !motor_manager_->stop()) { + std::cerr << "failed to stop motor manager during test cleanup" << std::endl; + } + } + +private: + std::shared_ptr motor_manager_; +}; + +class MultiMotorSafetyGuard { +public: + explicit MultiMotorSafetyGuard( + const std::vector>& motors) + : motors_(motors) + { + } + + ~MultiMotorSafetyGuard() + { + if (armed_) { + stop(); + } + } + + bool stop() + { + bool all_ok = true; + for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) { + if (*it && !(*it)->quickStop()) { + all_ok = false; + } + } + for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) { + if (*it && !(*it)->torqueOff()) { + all_ok = false; + } + } + armed_ = !all_ok; + return all_ok; + } + +private: + const std::vector>& motors_; + bool armed_{true}; +}; + struct TrackingErrorStats { std::int64_t sample_count{0}; double sum_error{0.0}; @@ -140,31 +201,16 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState) DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); auto motor_manager = device_manager.getDevice(kMotorManagerId); ASSERT_NE(motor_manager, nullptr); + MotorManagerStopGuard motor_manager_stop_guard(motor_manager); - // for (int motor_id = 1; motor_id <= 4; ++motor_id) { - // printMotorState(motor_id, motor_manager->getMotor(motor_id)); - // } - - auto motor = motor_manager->getMotor(4); - motor->calibrateZeroQ(); - - ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); - motor->commandProfileVelocity(-2,5); - - - for (int i = 1; i <= 50; ++i) - { - auto q = motor->getQ(); - auto qd = motor->getQd(); - std::cout << "q=" << q << ", qd=" << qd << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + for (const int motor_id : kFourMotorIds) { + auto motor = motor_manager->getMotor(static_cast(motor_id)); + ASSERT_NE(motor, nullptr); + printMotorState(motor_id, motor); } - motor->quickStop(); - } -TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) +TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory) { ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); DeviceManagerDestroyGuard guard; @@ -173,16 +219,21 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); auto motor_manager = device_manager.getDevice(kMotorManagerId); ASSERT_NE(motor_manager, nullptr); + MotorManagerStopGuard motor_manager_stop_guard(motor_manager); - std::array, kFourMotorIds.size()> motors; + std::vector> motors(kFourMotorIds.size()); 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]); } + MultiMotorSafetyGuard safety_guard(motors); std::cout << "calibrate zero for four EtherCAT motors" << std::endl; + for (const auto& motor : motors) { + ASSERT_TRUE(motor->torqueOff()); + } for (std::size_t i = 0; i < motors.size(); ++i) { const int motor_id = kFourMotorIds[i]; std::cout << "before calibrateZeroQ: "; @@ -196,48 +247,64 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) ASSERT_TRUE(motor->torqueOn()); } for (const auto& motor : motors) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + ASSERT_TRUE(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(); - } + std::vector center_q; + std::vector actual_qd; + ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd)); 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 + << " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad + << " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad << " rad, period=" << kFourMotorPeriodS << " s" << std::endl; const auto start_time = std::chrono::steady_clock::now(); - const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod; + const auto end_time = start_time + kFourMotorTrajectoryDuration; + auto next_command_time = start_time; + auto next_stats_time = start_time; + std::uint64_t missed_command_deadlines = 0; std::array error_stats; + std::vector target_q(motors.size(), 0.0); + std::vector target_qd(motors.size(), 0.0); + std::vector actual_q; + std::array minimum_actual_q{}; + std::array maximum_actual_q{}; + std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin()); + std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin()); 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; + while (true) { + std::this_thread::sleep_until(next_command_time); + const auto now = std::chrono::steady_clock::now(); + if (now > end_time) { + break; + } + const double t_s = std::chrono::duration(now - start_time).count(); - 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])); + target_q[i] = + center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta)); + target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta); } + ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd)); - if (elapsed.count() % kStatsSamplePeriod.count() == 0) { + if (now >= next_stats_time) { + ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd)); 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]; + const double error = actual_q[i] - target_q[i]; error_stats[i].add(error, theta); + minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]); + maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]); min_error = std::min(min_error, error); max_error = std::max(max_error, error); } @@ -245,32 +312,25 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) max_error_spread_rad = std::max(max_error_spread_rad, error_spread); sum_error_spread_sq += error_spread * error_spread; ++error_spread_sample_count; + next_stats_time = now + kStatsSamplePeriod; } - // 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)); + next_command_time += kCyclicCommandPeriod; + const auto command_complete_time = std::chrono::steady_clock::now(); + if (next_command_time <= command_complete_time) { + const auto skipped_periods = + (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1; + missed_command_deadlines += static_cast(skipped_periods); + next_command_time += skipped_periods * kCyclicCommandPeriod; } - std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod); } + std::copy(center_q.begin(), center_q.end(), target_q.begin()); + std::fill(target_qd.begin(), target_qd.end(), 0.0); + ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd)); + std::this_thread::sleep_for(kHoldAfterTrajectoryDuration); + ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd)); + std::cout << "after four motor CSP trajectory" << std::endl; for (std::size_t i = 0; i < motors.size(); ++i) { printMotorState(kFourMotorIds[i], motors[i]); @@ -284,6 +344,7 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) std::cout << "four motor CSP tracking error statistics, sample_period=" << kStatsSamplePeriod.count() << " ms, samples=" << error_stats.front().sample_count + << ", missed_command_deadlines=" << missed_command_deadlines << ", max_error_spread=" << max_error_spread_rad << " rad, rms_error_spread=" << rms_error_spread << " rad" << std::endl; @@ -303,7 +364,19 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) << " rad (" << radToDeg(relative_phase) << " deg, " << relative_phase_ms << " ms)" << std::endl; + EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i], + kMinimumPositionExcursionRad) + << "motor_id=" << kFourMotorIds[i] << " did not complete enough motion"; + EXPECT_LE(error_stats[i].max_abs_error, + kMaximumAbsoluteTrackingErrorRad) + << "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error"; + EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad) + << "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error"; + EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad) + << "motor_id=" << kFourMotorIds[i] << " did not return to its start position"; } + EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad); + EXPECT_TRUE(safety_guard.stop()); } } // 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 7753fda1..ecc9c007 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 @@ -23,7 +23,6 @@ namespace cmvr::device { namespace { constexpr int kMotorId = 1; -constexpr std::chrono::milliseconds kModeSettleDelay{100}; constexpr std::chrono::milliseconds kCommandSamplePeriod{100}; constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000}; @@ -119,10 +118,10 @@ config::MotorConfigItem createMotorConfig() config::MotorConfigItem config; config.set_id(kMotorId); config.set_joint_name("ethercat_test_joint"); - config.set_limit_q_lb(-6.14); - config.set_limit_q_ub(6.14); + config.set_limit_q_lb(-36.14); + config.set_limit_q_ub(36.14); config.set_limit_qd(10.0); - config.set_limit_qdd(10.0); + config.set_limit_qdd(100.0); config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev); config.set_gear_ratio(kDefaultGearRatio); return config; @@ -239,6 +238,9 @@ TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter) ASSERT_TRUE(motor->calibrateZeroQ()); printMotorState("after calibrateZeroQ", *motor); ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION)); + ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); } TEST(EyouMotorRealTest, CommandProfilePosition) @@ -251,8 +253,7 @@ TEST(EyouMotorRealTest, CommandProfilePosition) ASSERT_NE(motor, nullptr); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION)); ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0)); sampleMotorState(*motor, kFeedbackSampleDuration); @@ -267,9 +268,10 @@ TEST(EyouMotorRealTest, CommandProfileVelocity) auto motor = createMotor(runtime); ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY)); std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl; ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0)); @@ -289,21 +291,20 @@ TEST(EyouMotorRealTest, CommandCyclicPosition) auto motor = createMotor(runtime); ASSERT_NE(motor, nullptr); + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)); - const std::chrono::milliseconds trajectory_duration{15000}; + const std::chrono::milliseconds trajectory_duration{12000}; const double period_s = 6.0; - const double amplitude_rad = 3; - const double phase_rad = 0.0; + const double excursion_rad = 3.0; const double center_q = motor->getQ(); const double omega = 2.0 * kPi / period_s; - std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q + std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q << " rad, period=" << period_s - << " s, amplitude=" << amplitude_rad - << " rad, phase=" << phase_rad + << " s, excursion=" << excursion_rad << " rad, command_period=" << kCyclicCommandPeriod.count() << " ms" << std::endl; @@ -312,9 +313,11 @@ TEST(EyouMotorRealTest, CommandCyclicPosition) 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; - const double theta = omega * t_s + phase_rad; - const double target_q = center_q + amplitude_rad * std::sin(theta); - const double target_qd = amplitude_rad * omega * std::cos(theta); + const double theta = omega * t_s; + const double target_q = + center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta)); + const double target_qd = + 0.5 * excursion_rad * omega * std::sin(theta); ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); if (elapsed.count() % kCommandSamplePeriod.count() == 0) { @@ -337,19 +340,22 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity) auto motor = createMotor(runtime); ASSERT_NE(motor, nullptr); + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)); - const std::chrono::milliseconds trajectory_duration{15000}; - const double period_s = 6.0; - const double velocity_amplitude_rad_s = 5.0; + const std::chrono::milliseconds trajectory_duration{12000}; + const double period_s = 5.0; + const double excursion_rad = 5.0; const double phase_rad = 0.0; const double omega = 2.0 * kPi / period_s; + const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega; std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s << " s, velocity_amplitude=" << velocity_amplitude_rad_s - << " rad/s, phase=" << phase_rad + << " rad/s, excursion=" << excursion_rad + << " rad, phase=" << phase_rad << " rad, command_period=" << kCyclicCommandPeriod.count() << " ms" << std::endl; @@ -367,6 +373,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity) << " ms, target_qd=" << target_qd << " rad/s"; printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); } std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); } @@ -374,6 +381,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity) std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl; ASSERT_TRUE(motor->commandCyclicVelocity(0.0)); sampleMotorState(*motor, std::chrono::milliseconds{500}); + printRawEthercatFeedback("raw feedback after stop", runtime); } TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds) @@ -388,8 +396,7 @@ TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds) ASSERT_TRUE(motor->torqueOff()); ASSERT_TRUE(motor->calibrateZeroQ()); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)); const std::chrono::milliseconds run_duration{2000}; const double period_s = 6.0; @@ -444,8 +451,7 @@ TEST(EyouMotorRealTest, QuickStopInProfilePosition) ASSERT_TRUE(motor->torqueOff()); ASSERT_TRUE(motor->calibrateZeroQ()); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION)); const std::chrono::milliseconds quick_stop_time{1000}; const double start_q = motor->getQ(); @@ -484,8 +490,7 @@ TEST(EyouMotorRealTest, QuickStopInProfileVelocity) ASSERT_TRUE(motor->torqueOff()); ASSERT_TRUE(motor->calibrateZeroQ()); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY)); const std::chrono::milliseconds quick_stop_time{2000}; const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0; @@ -521,8 +526,7 @@ TEST(EyouMotorRealTest, QuickStopInCyclicPosition) ASSERT_TRUE(motor->torqueOff()); ASSERT_TRUE(motor->calibrateZeroQ()); ASSERT_TRUE(motor->torqueOn()); - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - std::this_thread::sleep_for(kModeSettleDelay); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)); const std::chrono::milliseconds run_duration{2000}; const double start_q = motor->getQ();