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(