Ignore the first 200 NANs when zeroing.
Change-Id: Ib181361546f5ebada699931130e31a414799cb28
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index d897fb5..c2de2cb 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -262,7 +262,14 @@
}
void PotAndAbsEncoderZeroingEstimator::Reset() {
+ first_offset_ = 0.0;
+ pot_relative_encoder_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ filtered_position_ = 0.0;
+ position_ = 0.0;
zeroed_ = false;
+ nan_samples_ = 0;
relative_to_absolute_offset_samples_.clear();
offset_samples_.clear();
buffered_samples_.clear();
@@ -291,7 +298,22 @@
// 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;
+ if (zeroed_) {
+ LOG(ERROR, "NAN on absolute encoder\n");
+ error_ = true;
+ } else {
+ ++nan_samples_;
+ LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
+ static_cast<int>(nan_samples_));
+ if (nan_samples_ >= constants_.average_filter_size) {
+ error_ = true;
+ zeroed_ = true;
+ }
+ }
+ // Throw some dummy values in for now.
+ filtered_absolute_encoder_ = info.absolute_encoder;
+ filtered_position_ = pot_relative_encoder_offset_ + info.encoder;
+ position_ = offset_ + info.encoder;
return;
}
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index cfac284..48caae0 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -258,7 +258,9 @@
double offset_ = 0;
// The next position in 'relative_to_absolute_offset_samples_' and
// 'encoder_samples_' to be used to store the next sample.
- int samples_idx_;
+ int samples_idx_ = 0;
+
+ size_t nan_samples_ = 0;
// The unzeroed filtered position.
double filtered_position_ = 0.0;
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 27ae179..549b3ed 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -350,6 +350,43 @@
EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
}
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwords.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ PotAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ PotAndAbsEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ PotAndAbsolutePosition sensor_values;
+ sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
+ sensor_values.encoder = 0.0;
+ sensor_values.pot = 0.0;
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(sensor_values);
+ }
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
// Makes sure that using an absolute encoder doesn't let us zero while moving.
TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithMovement) {
const double index_diff = 1.0;
@@ -384,12 +421,17 @@
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_);
+ PotAndAbsolutePosition sensor_values;
+ sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
+ sensor_values.encoder = 0.0;
+ sensor_values.pot = 0.0;
+ // We tolerate a couple NANs before we start.
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(sensor_values);
+ }
+ ASSERT_FALSE(estimator.error());
+ estimator.UpdateEstimate(sensor_values);
ASSERT_TRUE(estimator.error());
}