Detect slipping on the PulseIndexZeroingEstimator
This only works after zeroing, not during, but it's a great start.
Change-Id: Ib3c9797a456b4181b82b47263020ba2120faf3d5
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 4fe0236..d897fb5 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -7,7 +7,6 @@
#include "frc971/zeroing/wrap.h"
-
namespace frc971 {
namespace zeroing {
namespace {
@@ -26,7 +25,6 @@
Reset();
}
-
void PotAndIndexPulseZeroingEstimator::Reset() {
samples_idx_ = 0;
offset_ = 0;
@@ -307,12 +305,10 @@
buffered_samples_[buffered_samples_idx_] = info;
auto max_value =
::std::max_element(buffered_samples_.begin(), buffered_samples_.end(),
- compare_encoder)
- ->encoder;
+ compare_encoder)->encoder;
auto min_value =
::std::min_element(buffered_samples_.begin(), buffered_samples_.end(),
- compare_encoder)
- ->encoder;
+ compare_encoder)->encoder;
if (::std::abs(max_value - min_value) < constants_.zeroing_threshold) {
// Robot isn't moving, use middle sample to determine offsets.
moving = false;
@@ -494,7 +490,11 @@
StoreIndexPulseMaxAndMin(info);
const int index_pulse_count = IndexPulseCount();
if (index_pulse_count > constants_.index_pulse_count) {
- error_ = true;
+ if (!error_) {
+ LOG(ERROR, "Got more index pulses than expected. Got %d expected %d.\n",
+ index_pulse_count, constants_.index_pulse_count);
+ error_ = true;
+ }
}
// TODO(austin): Detect if the encoder or index pulse is unplugged.
@@ -505,6 +505,34 @@
constants_.known_index_pulse * constants_.index_difference -
min_index_position_;
zeroed_ = true;
+ } else if (zeroed_ && !error_) {
+ // Detect whether the index pulse is somewhere other than where we expect
+ // it to be. First we compute the position of the most recent index pulse.
+ double index_pulse_distance =
+ info.latched_encoder + offset_ - constants_.measured_index_position;
+ // Second we compute the position of the index pulse in terms of
+ // the index difference. I.e. if this index pulse is two pulses away from
+ // the index pulse that we know about then this number should be positive
+ // or negative two.
+ double relative_distance =
+ index_pulse_distance / constants_.index_difference;
+ // Now we compute how far away the measured index pulse is from the
+ // expected index pulse.
+ double error = relative_distance - ::std::round(relative_distance);
+ // This lets us check if the index pulse is within an acceptable error
+ // margin of where we expected it to be.
+ if (::std::abs(error) > constants_.allowable_encoder_error) {
+ LOG(ERROR,
+ "Encoder ticks out of range since last index pulse. known index "
+ "pulse: %f, expected index pulse: %f, actual index pulse: %f, "
+ "allowable error: %f\n",
+ constants_.measured_index_position,
+ round(relative_distance) * constants_.index_difference +
+ constants_.measured_index_position,
+ info.latched_encoder + offset_,
+ constants_.allowable_encoder_error * constants_.index_difference);
+ error_ = true;
+ }
}
position_ = info.encoder + offset_;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 91d5b8c..9d7cea1 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -276,8 +276,7 @@
using ZeroingConstants = constants::EncoderPlusIndexZeroingConstants;
using State = IndexEstimatorState;
- PulseIndexZeroingEstimator(
- const constants::EncoderPlusIndexZeroingConstants &constants)
+ PulseIndexZeroingEstimator(const ZeroingConstants &constants)
: constants_(constants) {
Reset();
}
@@ -313,7 +312,7 @@
int IndexPulseCount() const;
// Contains the physical constants describing the system.
- const constants::EncoderPlusIndexZeroingConstants constants_;
+ const ZeroingConstants constants_;
// The smallest position of all the index pulses.
double min_index_position_;
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index a4ba3d7..819fad0 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -400,6 +400,7 @@
constants.index_difference = 10.0;
constants.measured_index_position = 20.0;
constants.known_index_pulse = 1;
+ constants.allowable_encoder_error = 0.01;
PositionSensorSimulator sim(constants.index_difference);
@@ -447,6 +448,57 @@
estimator.GetEstimatorState().position);
}
+// Tests that we can detect when an index pulse occurs where we didn't expect
+// it to for the PulseIndexZeroingEstimator.
+TEST_F(ZeroingTest, TestRelativeEncoderSlipping) {
+ EncoderPlusIndexZeroingConstants constants;
+ constants.index_pulse_count = 3;
+ constants.index_difference = 10.0;
+ constants.measured_index_position = 20.0;
+ constants.known_index_pulse = 1;
+ constants.allowable_encoder_error = 0.05;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos =
+ constants.measured_index_position + 0.5 * constants.index_difference;
+
+ for (double direction : {1.0, -1.0}) {
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position);
+
+ PulseIndexZeroingEstimator estimator(constants);
+
+ // Zero the estimator.
+ MoveTo(&sim, &estimator, start_pos - 1 * constants.index_difference);
+ MoveTo(
+ &sim, &estimator,
+ start_pos - constants.index_pulse_count * constants.index_difference);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+
+ // We have a 5% allowable error so we slip a little bit each time and make
+ // sure that the index pulses are still accepted.
+ for (double error = 0.00;
+ ::std::abs(error) < constants.allowable_encoder_error;
+ error += 0.01 * direction) {
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position +
+ error * constants.index_difference);
+ MoveTo(&sim, &estimator, start_pos - constants.index_difference);
+ EXPECT_FALSE(estimator.error());
+ }
+
+ // As soon as we hit cross the error margin, we should trigger an error.
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position +
+ constants.allowable_encoder_error * 1.1 *
+ constants.index_difference * direction);
+ MoveTo(&sim, &estimator, start_pos - constants.index_difference);
+ ASSERT_TRUE(estimator.error());
+ }
+}
+
// Tests that an error is detected when the starting position changes too much.
TEST_F(ZeroingTest, TestHallEffectZeroing) {
constants::HallEffectZeroingConstants constants;