Make it so that superstructure filters out NaN cone_position's
It turns out that wpilib_interface can output NaN's sometimes for the
time-of-flight sensors. We don't want to propagate these to the
drivetrain auto-align.
Change-Id: I61858d2b2dcac51d1d54b9f22b62c4f9c56cd935
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index ece0790..4919cd6 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -132,7 +132,7 @@
break;
}
constexpr double kInvalidReading = 0.93;
- if (reading > kInvalidReading) {
+ if (reading > kInvalidReading || !std::isfinite(reading)) {
return std::nullopt;
}
const TimeOfFlight *calibration = CHECK_NOTNULL(
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 80e41f9..b555277 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -451,7 +451,6 @@
SetEnabled(true);
WaitUntilZeroed();
-
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -605,23 +604,44 @@
RunFor(chrono::seconds(1));
// Game piece position should not be populated when invalid.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
// And then send a valid cone position.
superstructure_plant_.set_cone_position(0.5);
RunFor(chrono::seconds(1));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
EXPECT_FLOAT_EQ(0.0, superstructure_status_fetcher_->game_piece_position());
superstructure_plant_.set_cone_position(0.1);
RunFor(chrono::seconds(1));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
EXPECT_FLOAT_EQ(0.2, superstructure_status_fetcher_->game_piece_position());
+
+ // Test that if wpilib_interface spews out invalid values that we don't
+ // attempt to convert them.
+ superstructure_plant_.set_cone_position(
+ std::numeric_limits<double>::quiet_NaN());
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
+ EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
+
+ superstructure_plant_.set_cone_position(
+ -std::numeric_limits<double>::infinity());
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
+ EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
}
class SuperstructureBeambreakTest