Added catch for NaN in the Absolute Encoder ZeroingEstimator
Change-Id: I4ae379931dc198f3742b5d161af7e48b88f40d00
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 171a1d1..22e3b3d 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -184,6 +184,13 @@
// update estimates based on those samples.
void PotAndAbsEncoderZeroingEstimator::UpdateEstimate(
const PotAndAbsolutePosition &info) {
+ // Check for Abs Encoder NaN value that would mess up the rest of the zeroing
+ // code below. NaN values are given when the Absolute Encoder is disconnected.
+ if (::std::isnan(info.absolute_encoder)) {
+ error_ = true;
+ return;
+ }
+
bool moving = true;
if (buffered_samples_.size() < constants_.moving_buffer_size) {
// Not enough samples to start determining if the robot is moving or not,