test(ethercat): strengthen real motor trajectory coverage

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
 #)

cmvr-es/common/math/support_functions_test.cpp

@@ -0,0 +1,34 @@
+#include <cstdint>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#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<std::int32_t>::min(),
+        std::numeric_limits<std::int32_t>::max(), period);
+    EXPECT_GE(distance, 0);
+    EXPECT_LE(distance, period / 2);
+}

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 <algorithm>
+#include <array>
 #include <chrono>
 #include <cstdint>
 #include <iostream>
@@ -73,17 +74,42 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
         return;
     }
 
-    EXPECT_TRUE(runtime.writePdo<std::uint16_t>(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000));
+    const std::array command_writes{
-    EXPECT_TRUE(runtime.writePdo<std::int8_t>(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0));
+        EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
+            1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
+        EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
+            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;
+    std::array feedback_reads{
-    EXPECT_TRUE(runtime.readPdo<std::uint16_t>(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword));
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            1, msgs::CIA402_STATUS_WORD_6041, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
+            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;
+    const auto statusword =
-    EXPECT_TRUE(runtime.readPdo<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
+        EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
+    const auto mode_display =
+        EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(feedback_reads[1]);
 
     std::cout << "CIA402 statusword: 0x" << std::hex << statusword
               << ", mode display: " << std::dec << static_cast<int>(mode_display)

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp

@@ -7,6 +7,8 @@
 #include <limits>
 #include <memory>
 #include <thread>
+#include <utility>
+#include <vector>
 
 #include <gtest/gtest.h>
 
@@ -24,14 +26,18 @@ constexpr const char* kMotorConfigFile =
     "devices/motor/ethercat_motors_four_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 kRaisedCosineCoefficientRad = 0.2;
-constexpr double kFourMotorPeriodS = 1.0;
+constexpr double kFourMotorPeriodS = 2.0;
 constexpr std::array<double, 4> 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<MotorManager> 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<MotorManager> motor_manager_;
+};
+
+class MultiMotorSafetyGuard {
+public:
+    explicit MultiMotorSafetyGuard(
+        const std::vector<std::shared_ptr<AbstractMotor>>& 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<std::shared_ptr<AbstractMotor>>& 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<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    // for (int motor_id = 1; motor_id <= 4; ++motor_id) {
+    for (const int motor_id : kFourMotorIds) {
-    //     printMotorState(motor_id, motor_manager->getMotor(motor_id));
+        auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
-    // }
+        ASSERT_NE(motor, nullptr);
-
+        printMotorState(motor_id, motor);
-    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));
     }
-    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<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
+    std::vector<std::shared_ptr<AbstractMotor>> 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<std::uint8_t>(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<double, kFourMotorIds.size()> center_q{};
+    std::vector<double> center_q;
-    for (std::size_t i = 0; i < motors.size(); ++i) {
+    std::vector<double> actual_qd;
-        center_q[i] = motors[i]->getQ();
+    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<TrackingErrorStats, kFourMotorIds.size()> error_stats;
+    std::vector<double> target_q(motors.size(), 0.0);
+    std::vector<double> target_qd(motors.size(), 0.0);
+    std::vector<double> actual_q;
+    std::array<double, kFourMotorIds.size()> minimum_actual_q{};
+    std::array<double, kFourMotorIds.size()> 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) {
+    while (true) {
-        const auto elapsed = tick * kCyclicCommandPeriod;
+        std::this_thread::sleep_until(next_command_time);
-        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const auto now = std::chrono::steady_clock::now();
+        if (now > end_time) {
+            break;
+        }
+        const double t_s = std::chrono::duration<double>(now - start_time).count();
 
-        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_q[i] =
-            target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
+                center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
-            ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
+            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<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];
+                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,31 +312,24 @@ 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) {
+        next_command_time += kCyclicCommandPeriod;
-        //     std::cout << "t=" << elapsed.count() << " ms" << std::endl;
+        const auto command_complete_time = std::chrono::steady_clock::now();
-        //     for (std::size_t i = 0; i < motors.size(); ++i) {
+        if (next_command_time <= command_complete_time) {
-        //         std::cout << "  motor_id=" << kFourMotorIds[i]
+            const auto skipped_periods =
-        //                   << ", target_q=" << target_q[i]
+                (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
-        //                   << " rad, target_qd=" << target_qd[i]
+            missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
-        //                   << " rad/s, q=" << motors[i]->getQ()
+            next_command_time += skipped_periods * kCyclicCommandPeriod;
-        //                   << " 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();
+    std::copy(center_q.begin(), center_q.end(), target_q.begin());
-    const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
+    std::fill(target_qd.begin(), target_qd.end(), 0.0);
-    for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
+    ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
-        for (std::size_t i = 0; i < motors.size(); ++i) {
+    std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
-            ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
-        }
-        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) {
@@ -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

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_lb(-36.14);
-    config.set_limit_q_ub(6.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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
-    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 excursion_rad = 3.0;
-    const double phase_rad = 0.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
+              << " s, excursion=" << excursion_rad
-              << " rad, phase=" << phase_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<double>(elapsed.count()) / 1000.0;
-        const double theta = omega * t_s + phase_rad;
+        const double theta = omega * t_s;
-        const double target_q = center_q + amplitude_rad * std::sin(theta);
+        const double target_q =
-        const double target_qd = amplitude_rad * omega * std::cos(theta);
+            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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
-    const std::chrono::milliseconds trajectory_duration{15000};
+    const std::chrono::milliseconds trajectory_duration{12000};
-    const double period_s = 6.0;
+    const double period_s = 5.0;
-    const double velocity_amplitude_rad_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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
-    std::this_thread::sleep_for(kModeSettleDelay);
 
     const std::chrono::milliseconds run_duration{2000};
     const double start_q = motor->getQ();