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/BUILD b/y2022/localizer/BUILD
index 9e8c3d4..1a57caf 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -25,6 +25,13 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "localizer_output_ts_fbs",
+ srcs = ["localizer_output.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
flatbuffer_cc_library(
name = "localizer_status_fbs",
srcs = [
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index ea80e73..2b3c72c 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -978,6 +978,7 @@
output_builder.add_x(model_based_.xytheta()(0));
output_builder.add_y(model_based_.xytheta()(1));
output_builder.add_theta(model_based_.xytheta()(2));
+ output_builder.add_zeroed(zeroer_.Zeroed());
const Eigen::Quaterniond &orientation = model_based_.orientation();
Quaternion quaternion;
quaternion.mutate_x(orientation.x());
diff --git a/y2022/localizer/localizer_output.fbs b/y2022/localizer/localizer_output.fbs
index 03abf19..b07278b 100644
--- a/y2022/localizer/localizer_output.fbs
+++ b/y2022/localizer/localizer_output.fbs
@@ -21,6 +21,9 @@
theta:double (id: 3);
// Current estimate of the robot's 3-D rotation.
orientation:Quaternion (id: 4);
+ // Whether we have zeroed the IMU (may go false if we observe a fault with the
+ // IMU).
+ zeroed:bool (id: 5);
}
root_type LocalizerOutput;
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;