From 74b80a92a72e737c483ec2a46ce433cf25ceddcc Mon Sep 17 00:00:00 2001 From: Josh Pieper Date: Thu, 31 Mar 2022 11:26:21 -0400 Subject: [PATCH] Keep around a 10ms filtered velocity, use it for capturing Errors in the initial captured velocity can be greatly magnified when the acceleration limited trajectory planner is involved. So in addition to using the filtered velocity, we also clamp it hard to 0.0 if under a configurable value. --- fw/bldc_servo.cc | 2 + fw/bldc_servo_position.h | 8 +++- fw/bldc_servo_structs.h | 11 ++++++ fw/test/bldc_servo_position_test.cc | 61 ++++++++++++++++++++++++++++- utils/dynamometer_drive.cc | 10 ++--- 5 files changed, 84 insertions(+), 8 deletions(-) diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index c787f675..044b55f0 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -962,6 +962,8 @@ class BldcServo::Impl { static_cast(velocity_filter_.size()); } + ISR_UpdateFilteredValue(status_.velocity, &status_.velocity_filt, 0.01f); + status_.unwrapped_position = (status_.unwrapped_position_raw >> 32) / motor_scale16_; diff --git a/fw/bldc_servo_position.h b/fw/bldc_servo_position.h index ce8bee79..7b833997 100644 --- a/fw/bldc_servo_position.h +++ b/fw/bldc_servo_position.h @@ -288,7 +288,13 @@ class BldcServoPosition { status->control_velocity = velocity; } else if (!status->control_position) { status->control_position = status->unwrapped_position_raw; - status->control_velocity = status->velocity; + + if (std::abs(status->velocity_filt) < + config->velocity_zero_capture_threshold) { + status->control_velocity = 0.0f; + } else { + status->control_velocity = status->velocity_filt; + } } if (!status->trajectory_done) { diff --git a/fw/bldc_servo_structs.h b/fw/bldc_servo_structs.h index 2ef4b9b3..99b71edd 100644 --- a/fw/bldc_servo_structs.h +++ b/fw/bldc_servo_structs.h @@ -152,6 +152,8 @@ struct BldcServoStatus { float velocity = 0.0f; float torque_Nm = 0.0f; + float velocity_filt = 0.0f; + SimplePI::State pid_d; SimplePI::State pid_q; PID::State pid_position; @@ -246,6 +248,8 @@ struct BldcServoStatus { a->Visit(MJ_NVP(velocity)); a->Visit(MJ_NVP(torque_Nm)); + a->Visit(MJ_NVP(velocity_filt)); + a->Visit(MJ_NVP(pid_d)); a->Visit(MJ_NVP(pid_q)); a->Visit(MJ_NVP(pid_position)); @@ -503,6 +507,12 @@ struct BldcServoConfig { // port's configured value shortly after startup. bool rezero_from_abs = false; + // When starting position control from the "stopped" state, the + // control velocity will be initialized from 'velocity_filt'. If + // the absolute value is less than this, then it will be treated as + // exactly 0.0. + float velocity_zero_capture_threshold = 0.05f; + // A bitfield that selects one of several things to emit from the // debug UART at full control rate. uint32_t emit_debug = 0; @@ -570,6 +580,7 @@ struct BldcServoConfig { a->Visit(MJ_NVP(velocity_filter_length)); a->Visit(MJ_NVP(cooldown_cycles)); a->Visit(MJ_NVP(rezero_from_abs)); + a->Visit(MJ_NVP(velocity_zero_capture_threshold)); a->Visit(MJ_NVP(emit_debug)); a->Visit(MJ_NVP(encoder_filter)); } diff --git a/fw/test/bldc_servo_position_test.cc b/fw/test/bldc_servo_position_test.cc index 8cdae35c..a9294614 100644 --- a/fw/test/bldc_servo_position_test.cc +++ b/fw/test/bldc_servo_position_test.cc @@ -48,6 +48,11 @@ struct Context { status.unwrapped_position_raw = to_raw(status.unwrapped_position); } + void set_velocity(float val) { + status.velocity = val; + status.velocity_filt = val; + } + int64_t to_raw(double val) const { return static_cast(val * motor_scale16 * (1ull << 32)); } @@ -83,6 +88,58 @@ BOOST_AUTO_TEST_CASE(StartupPositionCapture) { BOOST_TEST(ctx.status.control_velocity.value() == 0.0f); } +BOOST_AUTO_TEST_CASE(StartupVelocityCapture, + * boost::unit_test::tolerance(1e-2)) { + struct TestCase { + double input_velocity; + double capture_threshold; + + double expected_capture; + }; + + TestCase test_cases[] = { + // a variance of 0.001 is a stddev of 0.0316, 6x that is 0.190 + { 0.0, 0.1, 0.00, }, + { 1.0, 0.1, 1.00, }, + {-1.0, 0.1, -1.00, }, + { 0.01, 0.1, 0.00, }, + { 0.09, 0.1, 0.00, }, + { 0.11, 0.1, 0.11, }, + {-0.09, 0.1, 0.00, }, + {-0.11, 0.1, -0.11, }, + + { 0.11, 0.2, 0.00, }, + { 0.19, 0.2, 0.00, }, + { 0.21, 0.2, 0.21, }, + + // and with the threshold set to 0, anything is captured + { 0.01, 0.0, 0.01 }, + {-0.01, 0.0, -0.01 }, + }; + + for (const auto& test_case : test_cases) { + BOOST_TEST_CONTEXT("Case " + << test_case.input_velocity << " " + << test_case.capture_threshold) { + Context ctx; + + ctx.data.position = 10.0; + ctx.data.velocity = 0.0; + ctx.data.accel_limit = 1.0f; + ctx.data.velocity_limit = 1.0f; + + ctx.status.velocity = test_case.input_velocity; + ctx.status.velocity_filt = test_case.input_velocity; + ctx.config.velocity_zero_capture_threshold = test_case.capture_threshold; + + BOOST_TEST(!ctx.status.control_velocity); + ctx.Call(); + BOOST_TEST(ctx.status.control_velocity.value() == + test_case.expected_capture); + } + } +} + BOOST_AUTO_TEST_CASE(StartupPositionSet) { // When starting, if a command position is given, we use that. Context ctx; @@ -359,7 +416,7 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) { ctx.data.accel_limit = test_case.a; ctx.data.velocity_limit = test_case.v; ctx.set_position(test_case.x0); - ctx.status.velocity = test_case.v0; + ctx.set_velocity(test_case.v0); double old_vel = test_case.v0; double old_pos = test_case.x0; @@ -494,7 +551,7 @@ BOOST_AUTO_TEST_CASE(StopPositionWithLimitOvershoot, * boost::unit_test::toleran ctx.data.velocity = 1.0f; ctx.data.accel_limit = 2.0f; ctx.data.velocity_limit = 3.0f; - ctx.status.velocity = 2.0f; + ctx.set_velocity(2.0f); ctx.set_position(0.0f); // Here, we'll get stopped at 0.2 as try to slow down and come back diff --git a/utils/dynamometer_drive.cc b/utils/dynamometer_drive.cc index 99dd7120..980c4ac5 100644 --- a/utils/dynamometer_drive.cc +++ b/utils/dynamometer_drive.cc @@ -209,7 +209,7 @@ class ServoStatsReader { // Here we verify that the final and total timer are always valid. if (result.final_timer == 0 || result.total_timer == 0 || - result.final_timer > 2970 || + result.final_timer > 3000 || result.total_timer < 4250) { throw mjlib::base::system_error::einval( fmt::format("Invalid timer final={} total={}", @@ -1630,9 +1630,9 @@ class Application { const auto spinning_dut = dut_->servo_stats(); const auto measured_speed = spinning_dut.unwrapped_position - initial_dut.unwrapped_position; - if (std::abs(measured_speed - kDesiredSpeed) > 0.05) { + if (std::abs(measured_speed - kDesiredSpeed) > 0.052) { throw mjlib::base::system_error::einval( - fmt::format("Base speed not achieved |{} - {}| > 0.05", + fmt::format("Base speed not achieved |{} - {}| > 0.052", measured_speed, kDesiredSpeed)); } @@ -1657,9 +1657,9 @@ class Application { const auto slow_speed = (slow_dut.unwrapped_position - spinning_dut.unwrapped_position) / 2.0; - if (std::abs(slow_speed - 0.5 * kDesiredSpeed) > 0.05) { + if (std::abs(slow_speed - 0.5 * kDesiredSpeed) > 0.052) { throw mjlib::base::system_error::einval( - fmt::format("DUT did not slow down |{} - {}| > 0.05", + fmt::format("DUT did not slow down |{} - {}| > 0.052", slow_speed, 0.5 * kDesiredSpeed)); }