Adding field plot for 2022
Adds some bare minimum status information, mostly focused on getting the
camera readings showing on the screen.
Note that this code is largely borrowed from the 2020 field viewing
code.
TODO: Add debug information for superstructure state machine/catapult.
Change-Id: I896a3def0d8d8b0ef2fb132086eaba1ba8ec0c61
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 58df869..3c11dff 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -398,12 +398,20 @@
}
{
auto builder = turret_sender_.MakeBuilder();
+ auto turret_estimator_builder =
+ builder
+ .MakeBuilder<frc971::PotAndAbsoluteEncoderEstimatorState>();
+ turret_estimator_builder.add_position(turret_position_);
+ const flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ turret_estimator_offset = turret_estimator_builder.Finish();
auto turret_builder =
builder
.MakeBuilder<frc971::control_loops::
PotAndAbsoluteEncoderProfiledJointStatus>();
turret_builder.add_position(turret_position_);
turret_builder.add_velocity(turret_velocity_);
+ turret_builder.add_zeroed(true);
+ turret_builder.add_estimator_state(turret_estimator_offset);
const auto turret_offset = turret_builder.Finish();
auto status_builder =
builder
@@ -784,6 +792,53 @@
status_fetcher_->model_based()->statistics()->total_accepted());
}
+// Tests that image corrections are ignored when the turret moves too fast.
+TEST_F(EventLoopLocalizerTest, ImageCorrectionsTurretTooFast) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.mutable_state()->x() = 2.0;
+ drivetrain_plant_.mutable_state()->y() = 2.0;
+ SendLocalizerControl(5.0, 3.0, 0.0);
+ turret_velocity_ = 10.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(output_fetcher_.Fetch());
+ ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5);
+ ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
+
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ CHECK(output_fetcher_.Fetch());
+ ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5);
+ ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
+ ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
+ ASSERT_LT(10,
+ status_fetcher_->model_based()->statistics()->total_candidates());
+ ASSERT_EQ(0, status_fetcher_->model_based()->statistics()->total_accepted());
+ ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(),
+ status_fetcher_->model_based()
+ ->statistics()
+ ->rejection_reason_count()
+ ->Get(static_cast<int>(RejectionReason::TURRET_TOO_FAST)));
+ // We expect one more rejection to occur due to the time it takes all the
+ // information to propagate.
+ const int rejected_count =
+ status_fetcher_->model_based()->statistics()->total_candidates() + 1;
+ // Check that when we go back to being still we do successfully converge.
+ turret_velocity_ = 0.0;
+ turret_position_ = 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-1));
+ ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
+ ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(),
+ rejected_count +
+ status_fetcher_->model_based()->statistics()->total_accepted());
+}
+
// Tests that image corrections when we are in accel mode works.
TEST_F(EventLoopLocalizerTest, ImageCorrectionsInAccel) {
output_voltages_ << 0.0, 0.0;