Add error detection to the absolute encoder zeroing
Change-Id: I36ca16ce174c0bd459f2d649fdae607b5813f2c5
diff --git a/frc971/constants.h b/frc971/constants.h
index 57637dd..7b4cb47 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -38,12 +38,15 @@
// Measured absolute position of the encoder when at zero.
double measured_absolute_position;
- // Treshold for deciding if we are moving
- // TODO(austin): Figure out what this is actually measuring.
+ // Treshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
double zeroing_threshold;
-
// Buffer size for deciding if we are moving.
size_t moving_buffer_size;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
};
// Defines a range of motion for a subsystem.
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 70fbb56..a1fd17e 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -34,7 +34,6 @@
zeroed_ = false;
wait_for_index_pulse_ = true;
last_used_index_pulse_count_ = 0;
- first_start_pos_ = 0.0;
error_ = false;
}
@@ -157,6 +156,7 @@
relative_to_absolute_offset_samples_.clear();
offset_samples_.clear();
buffered_samples_.clear();
+ error_ = false;
}
// So, this needs to be a multistep process. We need to first estimate the
@@ -292,6 +292,21 @@
constants_.one_revolution_distance) -
buffered_samples_[middle_index].encoder;
if (offset_ready()) {
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
+
+ if (::std::abs(first_offset_ - offset_) >
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance) {
+ LOG(ERROR,
+ "Offset moved too far. Initial: %f, current %f, allowable change: "
+ "%f\n",
+ first_offset_, offset_, constants_.allowable_encoder_error *
+ constants_.one_revolution_distance);
+ error_ = true;
+ }
+
zeroed_ = true;
}
}
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 53b8e31..689795f 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -164,8 +164,15 @@
private:
// The zeroing constants used to describe the configuration of the system.
const constants::PotAndAbsoluteEncoderZeroingConstants constants_;
+
// True if the mechanism is zeroed.
bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+ // The first valid offset we recorded. This is only set after zeroed_ first
+ // changes to true.
+ double first_offset_;
+
// Samples of the offset needed to line the relative encoder up with the
// absolute encoder.
::std::vector<double> relative_to_absolute_offset_samples_;
@@ -183,12 +190,11 @@
// The next position in 'relative_to_absolute_offset_samples_' and
// 'encoder_samples_' to be used to store the next sample.
int samples_idx_;
+
// The unzeroed filtered position.
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;
};
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 6373056..3e8c931 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -289,16 +289,8 @@
ASSERT_FALSE(estimator.error());
}
-// I want to test that the the zeroing class can
-// detect an error when the starting position
-// changes too much. I do so by creating the
-// simulator at an 'X' positon, making sure
-// that the estimator is zeroed, and then
-// initializing the simulator at another
-// position. After making sure it's zeroed,
-// if the error() function returns true,
-// then, it works.
-TEST_F(ZeroingTest, TestOffsetError) {
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(ZeroingTest, TestIndexOffsetError) {
const double index_diff = 0.8;
const double known_index_pos = 2 * index_diff;
const size_t sample_size = 30;
@@ -329,9 +321,9 @@
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};
+ 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);
@@ -356,9 +348,9 @@
const double start_pos = 10 * index_diff;
double measured_absolute_position = 0.3 * index_diff;
- PotAndAbsoluteEncoderZeroingConstants constants{kSampleSize, index_diff,
- measured_absolute_position,
- 0.1, kMovingBufferSize};
+ 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);
@@ -378,7 +370,7 @@
// 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};
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
PotAndAbsEncoderZeroingEstimator estimator(constants);
@@ -391,9 +383,7 @@
ASSERT_TRUE(estimator.error());
}
-// Makes sure that using only a relative encoder with index pulses allows us to
-// successfully zero.
-// We pretend that there are index pulses at 10, 20, and 30.
+// Tests that an error is detected when the starting position changes too much.
TEST_F(ZeroingTest, TestRelativeEncoderZeroing) {
EncoderPlusIndexZeroingConstants constants;
constants.index_pulse_count = 3;
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 35fe207..c716ecf 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -74,12 +74,14 @@
intake->zeroing.measured_absolute_position = 0;
intake->zeroing.zeroing_threshold = 0.001;
intake->zeroing.moving_buffer_size = 9;
+ intake->zeroing.allowable_encoder_error = 0.3;
turret->zeroing.average_filter_size = Values::kZeroingSampleSize;
turret->zeroing.one_revolution_distance = Values::kTurretEncoderIndexDifference;
turret->zeroing.measured_absolute_position = 0;
turret->zeroing.zeroing_threshold = 0.001;
turret->zeroing.moving_buffer_size = 9;
+ turret->zeroing.allowable_encoder_error = 0.3;
hood->zeroing.average_filter_size = Values::kZeroingSampleSize;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;