diff --git a/docs/reference.md b/docs/reference.md index efb1463c..0e664b01 100644 --- a/docs/reference.md +++ b/docs/reference.md @@ -294,6 +294,8 @@ to write. - 11 => timeout - 12 => zero velocity - 13 => stay within +- 14 => measure inductance +- 15 => brake #### 0x001 - Position #### @@ -793,6 +795,11 @@ The fields have the same semantics as for the register protocol documented above. The options are the same as for `d pos`, with the exception of stop position which is not supported. +### `d brake` ### + +Enter the "brake" state. In this mode, all motor phases are shorted +to ground, resulting in a passive "braking" action. + ### `d index` ### Force the current recorded position to match exactly the given value. diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index 61a8b473..f28a8b75 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -1066,7 +1066,8 @@ class BldcServo::Impl { case kPositionTimeout: case kZeroVelocity: case kStayWithinBounds: - case kMeasureInductance: { + case kMeasureInductance: + case kBrake: { return true; } } @@ -1103,7 +1104,8 @@ class BldcServo::Impl { case kPositionTimeout: case kZeroVelocity: case kStayWithinBounds: - case kMeasureInductance: { + case kMeasureInductance: + case kBrake: { switch (status_.mode) { case kNumModes: { MJ_ASSERT(false); @@ -1134,7 +1136,8 @@ class BldcServo::Impl { case kPosition: case kZeroVelocity: case kStayWithinBounds: - case kMeasureInductance: { + case kMeasureInductance: + case kBrake: { if ((data->mode == kPosition || data->mode == kStayWithinBounds) && ISR_IsOutsideLimits()) { status_.mode = kFault; @@ -1210,6 +1213,7 @@ class BldcServo::Impl { case kVoltageFoc: case kVoltageDq: case kMeasureInductance: + case kBrake: return false; case kCurrent: case kPosition: @@ -1245,6 +1249,7 @@ class BldcServo::Impl { case kVoltageDq: case kCurrent: case kMeasureInductance: + case kBrake: return false; case kPosition: case kPositionTimeout: @@ -1388,6 +1393,10 @@ class BldcServo::Impl { ISR_DoMeasureInductance(sin_cos, data); break; } + case kBrake: { + ISR_DoBrake(); + break; + } } } @@ -1981,6 +1990,14 @@ class BldcServo::Impl { ISR_DoBalancedVoltageControl(ISR_CalculatePhaseVoltage(sin_cos, d_V, 0.0f)); } + void ISR_DoBrake() MOTEUS_CCM_ATTRIBUTE { + *pwm1_ccr_ = 0; + *pwm2_ccr_ = 0; + *pwm3_ccr_ = 0; + + motor_driver_->Power(true); + } + void ISR_MaybeEmitDebug() MOTEUS_CCM_ATTRIBUTE { if (config_.emit_debug == 0) { return; } diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h index 3396b422..6ad311c1 100644 --- a/fw/bldc_servo.h +++ b/fw/bldc_servo.h @@ -365,6 +365,9 @@ class BldcServo { // approximately equal D/Q axis inductances. kMeasureInductance = 14, + // All phases are pulled to ground. + kBrake = 15, + kNumModes, }; @@ -693,6 +696,7 @@ struct IsEnum { { M::kZeroVelocity, "zero_vel" }, { M::kStayWithinBounds, "within" }, { M::kMeasureInductance, "meas_ind" }, + { M::kBrake, "brake" }, }}; } }; diff --git a/fw/board_debug.cc b/fw/board_debug.cc index d792eb22..2cd7aa8e 100644 --- a/fw/board_debug.cc +++ b/fw/board_debug.cc @@ -504,6 +504,14 @@ class BoardDebug::Impl { return; } + if (cmd_text == "brake") { + BldcServo::CommandData command; + command.mode = BldcServo::kBrake; + bldc_->Command(command); + WriteOk(response); + return; + } + if (cmd_text == "index") { const auto pos_value = tokenizer.next(); if (pos_value.empty()) { diff --git a/utils/dynamometer_drive.cc b/utils/dynamometer_drive.cc index 5902d9e1..5d526acb 100644 --- a/utils/dynamometer_drive.cc +++ b/utils/dynamometer_drive.cc @@ -94,6 +94,7 @@ struct Options { bool validate_rezero = false; bool validate_voltage_mode_control = false; bool validate_fixed_voltage_mode = false; + bool validate_brake_mode = false; template void Serialize(Archive* a) { @@ -127,6 +128,7 @@ struct Options { a->Visit(MJ_NVP(validate_rezero)); a->Visit(MJ_NVP(validate_voltage_mode_control)); a->Visit(MJ_NVP(validate_fixed_voltage_mode)); + a->Visit(MJ_NVP(validate_brake_mode)); } }; @@ -635,6 +637,8 @@ class Application { co_await ValidateVoltageModeControl(); } else if (options_.validate_fixed_voltage_mode) { co_await ValidateFixedVoltageMode(); + } else if (options_.validate_brake_mode) { + co_await ValidateBrakeMode(); } else { fmt::print("No cycle selected\n"); } @@ -2060,6 +2064,47 @@ class Application { co_return; } + boost::asio::awaitable ValidateBrakeMode() { + co_await dut_->Command("d stop"); + co_await fixture_->Command("d stop"); + co_await fixture_->Command("d index 0"); + + co_await dut_->Command("d brake"); + co_await CommandFixtureRigid(); + + struct BrakeTest { + double velocity; + double expected_torque; + } brake_tests[] = { + // These torques were just experimentally determined on the dyno + // fixture's MiToot motor. + { 0.0, 0.0 }, + { 2.0, 0.1 }, + { 5.0, 0.25 }, + { -2.0, -0.1 }, + { -5.0, -0.25 }, + }; + + for (const auto& test : brake_tests) { + co_await fixture_->Command( + fmt::format("d pos nan {} {}", + test.velocity, + options_.max_torque_Nm)); + co_await Sleep(1.0); + + if (std::abs(current_torque_Nm_ - test.expected_torque) > 0.05) { + throw mjlib::base::system_error::einval( + fmt::format("brake torque not as expected {} != {} (within {})", + current_torque_Nm_, test.expected_torque, 0.05)); + } + } + + co_await dut_->Command("d stop"); + co_await fixture_->Command("d stop"); + + 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 e48e8583..bf802dee 100644 --- a/utils/firmware_validate.py +++ b/utils/firmware_validate.py @@ -103,6 +103,9 @@ def test_validate_voltage_mode_control(self): def test_validate_fixed_voltage_mode(self): dyno('--validate_fixed_voltage_mode', '1') + def test_validate_brake_mode(self): + dyno('--validate_brake_mode', '1', keep_log=True) + class TestDynoSlow(unittest.TestCase): def test_torque_ripple(self):