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,
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index dc8a1ad..d850f54 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -155,8 +155,7 @@
double offset() const override { return offset_; }
- // TODO(austin): Actually implement this.
- bool error() const override { return false; }
+ bool error() const override { return error_; }
// Returns true if the sample buffer is full.
bool offset_ready() const {
@@ -195,6 +194,8 @@
double filtered_position_ = 0.0;
// The filtered position.
double position_ = 0.0;
+ // Whether or not there is an error in the estimate.
+ bool error_ = false;
};
// Populates an EstimatorState struct with information from the zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 420ca61..526a514 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -38,7 +38,8 @@
}
void MoveTo(PositionSensorSimulator *simulator,
- PotAndAbsEncoderZeroingEstimator *estimator, double new_position) {
+ PotAndAbsEncoderZeroingEstimator *estimator,
+ double new_position) {
PotAndAbsolutePosition sensor_values_;
simulator->MoveTo(new_position);
simulator->GetSensorValues(&sensor_values_);
@@ -365,5 +366,22 @@
MoveTo(&sim, &estimator, start_pos);
ASSERT_FALSE(estimator.zeroed());
}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
+ PotAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize};
+
+ PotAndAbsEncoderZeroingEstimator estimator(constants);
+
+ PotAndAbsolutePosition sensor_values_;
+ sensor_values_.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
+ sensor_values_.encoder = 0.0;
+ sensor_values_.pot = 0.0;
+ estimator.UpdateEstimate(sensor_values_);
+
+ ASSERT_TRUE(estimator.error());
+}
+
} // namespace zeroing
} // namespace frc971