Skip to content

Commit

Permalink
Add a basic test for fixed voltage mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@jpieper
jpieper committed Dec 3, 2021
1 parent 501b886 commit 08bdd6d
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 11 deletions.
71 changes: 61 additions & 10 deletions utils/dynamometer_drive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename Archive>
void Serialize(Archive* a) {
Expand Down Expand Up @@ -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));
}
};

Expand Down Expand Up @@ -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<void> ConfigurePid(const PidConstants& pid) {
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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");
}
Expand Down Expand Up @@ -1025,6 +1038,10 @@ class Application {
}
}

co_await RunBasicPositionVelocityTest(pid, 1.0);
}

boost::asio::awaitable<void> 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);
Expand All @@ -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));
Expand All @@ -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));
Expand All @@ -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));
Expand All @@ -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",
Expand All @@ -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);
Expand All @@ -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);
}
Expand Down Expand Up @@ -2009,6 +2028,38 @@ class Application {
co_return;
}

boost::asio::awaitable<void> 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<void> Sleep(double seconds) {
boost::asio::deadline_timer timer(executor_);
timer.expires_from_now(mjlib::base::ConvertSecondsToDuration(seconds));
Expand Down
5 changes: 4 additions & 1 deletion utils/firmware_validate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down

0 comments on commit 08bdd6d

Please sign in to comment.