From 08bdd6d18b736c022bf3d17a011c118a9dfaac28 Mon Sep 17 00:00:00 2001 From: Josh Pieper Date: Fri, 3 Dec 2021 12:08:39 -0500 Subject: [PATCH] Add a basic test for fixed voltage mode --- utils/dynamometer_drive.cc | 71 ++++++++++++++++++++++++++++++++------ utils/firmware_validate.py | 5 ++- 2 files changed, 65 insertions(+), 11 deletions(-) diff --git a/utils/dynamometer_drive.cc b/utils/dynamometer_drive.cc index 8ced5d7f..dc48daaa 100644 --- a/utils/dynamometer_drive.cc +++ b/utils/dynamometer_drive.cc @@ -93,6 +93,7 @@ struct Options { bool validate_max_velocity = false; bool validate_rezero = false; bool validate_voltage_mode_control = false; + bool validate_fixed_voltage_mode = false; template void Serialize(Archive* a) { @@ -125,6 +126,7 @@ struct Options { a->Visit(MJ_NVP(validate_max_velocity)); a->Visit(MJ_NVP(validate_rezero)); a->Visit(MJ_NVP(validate_voltage_mode_control)); + a->Visit(MJ_NVP(validate_fixed_voltage_mode)); } }; @@ -349,6 +351,9 @@ class Controller { double max_velocity = 500.0; bool voltage_mode_control = false; + + bool fixed_voltage_mode = false; + double fixed_voltage_control_V = 0.0; }; boost::asio::awaitable ConfigurePid(const PidConstants& pid) { @@ -380,6 +385,12 @@ class Controller { co_await Command( fmt::format("conf set servo.voltage_mode_control {}", pid.voltage_mode_control ? 1 : 0)); + co_await Command( + fmt::format("conf set servo.fixed_voltage_mode {}", pid.fixed_voltage_mode ? 1 : 0)); + + co_await Command( + fmt::format("conf set servo.fixed_voltage_control_V {}", pid.fixed_voltage_control_V)); + co_return; } @@ -622,6 +633,8 @@ class Application { co_await ValidateRezero(); } else if (options_.validate_voltage_mode_control) { co_await ValidateVoltageModeControl(); + } else if (options_.validate_fixed_voltage_mode) { + co_await ValidateFixedVoltageMode(); } else { fmt::print("No cycle selected\n"); } @@ -1025,6 +1038,10 @@ class Application { } } + co_await RunBasicPositionVelocityTest(pid, 1.0); + } + + boost::asio::awaitable RunBasicPositionVelocityTest(Controller::PidConstants pid, double tolerance_scale) { // Move at a few different velocities. for (const double velocity : {0.0, -1.5, 3.0}) { fmt::print("Moving at velocity {}\n", velocity); @@ -1034,7 +1051,7 @@ class Application { const double fixture_velocity = options_.transducer_scale * fixture_->servo_stats().velocity; - if (std::abs(fixture_velocity - velocity) > 0.35) { + if (std::abs(fixture_velocity - velocity) > 0.35 * tolerance_scale) { throw mjlib::base::system_error::einval( fmt::format("Fixture velocity {} != {}", fixture_velocity, velocity)); @@ -1054,7 +1071,7 @@ class Application { co_await Sleep(0.3); const double fixture_velocity = options_.transducer_scale * fixture_->servo_stats().velocity; - if ((std::abs(fixture_velocity) - kFixedVelocity) > 0.35) { + if ((std::abs(fixture_velocity) - kFixedVelocity) > 0.35 * tolerance_scale) { throw mjlib::base::system_error::einval( fmt::format("Fixture velocity {} != {}", fixture_velocity, kFixedVelocity)); @@ -1064,7 +1081,7 @@ class Application { co_await Sleep(2.5); const double fixture_position = options_.transducer_scale * fixture_->servo_stats().unwrapped_position; - if (std::abs(fixture_position - stop_position) > 0.07) { + if (std::abs(fixture_position - stop_position) > 0.07 * tolerance_scale) { throw mjlib::base::system_error::einval( fmt::format("Fixture stop position {} != {}", fixture_position, stop_position)); @@ -1074,13 +1091,16 @@ class Application { // Configure with some position limits in place and verify we // don't move outside of them by much. for (const double position_limit : {0.1, 1.0, 2.0}) { + co_await dut_->Command( + fmt::format("d pos nan 1.5 {} s0", options_.max_torque_Nm)); + co_await Sleep(3.0); + co_await fixture_->Command("d index 0"); + fmt::print("Testing position limit {}\n", position_limit); auto pid_limit = pid; pid_limit.position_min = -position_limit; pid_limit.position_max = 10.0; co_await dut_->ConfigurePid(pid_limit); - co_await dut_->Command("d index 0"); - co_await fixture_->Command("d index 0"); co_await dut_->Command( fmt::format("d pos nan 1.5 {} s-10", @@ -1090,14 +1110,13 @@ class Application { { const double fixture_position = options_.transducer_scale * fixture_->servo_stats().unwrapped_position; - if (std::abs(fixture_position - (-position_limit)) > 0.07) { + if (std::abs(fixture_position - (-position_limit)) > 0.07 * tolerance_scale) { throw mjlib::base::system_error::einval( fmt::format("Fixture stop position {} != {}", fixture_position, -position_limit)); } } - co_await dut_->Command("d stop"); pid_limit.position_min = -10.0; pid_limit.position_max = position_limit; co_await dut_->ConfigurePid(pid_limit); @@ -1108,16 +1127,16 @@ class Application { { const double fixture_position = options_.transducer_scale * fixture_->servo_stats().unwrapped_position; - if (std::abs(fixture_position - position_limit) > 0.05) { + if (std::abs(fixture_position - position_limit) > 0.05 * tolerance_scale) { throw mjlib::base::system_error::einval( fmt::format("Fixture stop position {} != {}", fixture_position, position_limit)); } } - - co_await dut_->Command("d stop"); } + co_await dut_->Command("d stop"); + // Get back to our default config. co_await dut_->ConfigurePid(pid); } @@ -2009,6 +2028,38 @@ class Application { co_return; } + boost::asio::awaitable ValidateFixedVoltageMode() { + co_await dut_->Command("d stop"); + co_await fixture_->Command("d stop"); + co_await dut_->Command("d index 0"); + + Controller::PidConstants pid; + pid.voltage_mode_control = true; + pid.kp = 1.0; + pid.ki = 0.0; + pid.kd = 0.01; + pid.fixed_voltage_mode = true; + pid.fixed_voltage_control_V = 0.45; + + co_await dut_->ConfigurePid(pid); + + // In this mode, the DUT ignores the encoder, so when we turn it + // on to begin with, it will center on a random position. So turn + // it on first, then zero the fixture. + co_await dut_->Command("d pos 0 0 0.2"); + co_await Sleep(0.5); + co_await fixture_->Command("d index 0"); + + // Despite burning power, all the basic position mode things that + // don't involve jumps should work as is with fixed voltage mode. + co_await RunBasicPositionVelocityTest(pid, 1.9); + + // However, we can use the fixture to drive the motor to the next + // electrical phase and then it will stay there. + + co_return; + } + boost::asio::awaitable Sleep(double seconds) { boost::asio::deadline_timer timer(executor_); timer.expires_from_now(mjlib::base::ConvertSecondsToDuration(seconds)); diff --git a/utils/firmware_validate.py b/utils/firmware_validate.py index ebc8e26c..e48e8583 100644 --- a/utils/firmware_validate.py +++ b/utils/firmware_validate.py @@ -97,9 +97,12 @@ def test_validate_max_velocity(self): def test_rezero(self): dyno('--validate_rezero', '1') - def test_voltage_mode_control(self): + def test_validate_voltage_mode_control(self): dyno('--validate_voltage_mode_control', '1') + def test_validate_fixed_voltage_mode(self): + dyno('--validate_fixed_voltage_mode', '1') + class TestDynoSlow(unittest.TestCase): def test_torque_ripple(self):