Added detection of bad index zeroing pulse.
Change-Id: I1607ce0d37b336136a4a278afc8a0f2897d1c597
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 9b2347d..d8c77e8 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -6,8 +6,8 @@
namespace frc971 {
namespace zeroing {
-void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
- EstimatorState *state) {
+void PopulateEstimatorState(const zeroing::ZeroingEstimator& estimator,
+ EstimatorState* state) {
state->error = estimator.error();
state->zeroed = estimator.zeroed();
state->position = estimator.position();
@@ -18,9 +18,8 @@
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
known_index_pos_ = constants.measured_index_position;
-
+ allowable_encoder_error_ = constants.allowable_encoder_error;
start_pos_samples_.reserve(max_sample_count_);
-
Reset();
}
@@ -31,6 +30,7 @@
zeroed_ = false;
wait_for_index_pulse_ = true;
last_used_index_pulse_count_ = 0;
+ first_start_pos_ = 0.0;
error_ = false;
}
@@ -93,10 +93,28 @@
// resilient to corrupted intermediate data.
start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
last_used_index_pulse_count_ = info.index_pulses;
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = start_pos_;
+ LOG(INFO, "latching start position %f\n", first_start_pos_);
+ }
// Now that we have an accurate starting position we can consider ourselves
// zeroed.
zeroed_ = true;
+ // Throw an error if first_start_pos is bigger/smaller than
+ // allowable_encoder_error_ * index_diff +
+ // start_pos.
+ if (::std::abs(first_start_pos_ - start_pos_) >
+ allowable_encoder_error_ * index_diff_) {
+ if (!error_) {
+ LOG(ERROR,
+ "Encoder ticks out of range since last index pulse. first start "
+ "position: %f recent starting position: %f\n",
+ first_start_pos_, start_pos_);
+ error_ = true;
+ }
+ }
}
pos_ = start_pos_ + info.encoder;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 7fa2262..08911ac 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -102,6 +102,12 @@
// Marker to track whether an error has occurred. This gets reset to false
// whenever Reset() is called.
bool error_;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+ // Value between 0 and 1 which determines a fraction of the index_diff
+ // you want to use.
+ double allowable_encoder_error_;
};
// Populates an EstimatorState struct with information from the zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 2ef6865..404b0e0 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -20,6 +20,7 @@
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
+static const double kIndexErrorFraction = 0.3;
class ZeroingTest : public ::testing::Test {
protected:
@@ -40,8 +41,8 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
@@ -64,8 +65,8 @@
double position = 3.6 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(position, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure that the zeroing code does not consider itself zeroed until we
// collect a good amount of samples. In this case we're waiting until the
@@ -83,8 +84,8 @@
double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
@@ -116,8 +117,8 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
@@ -150,8 +151,8 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
for (unsigned int i = 0; i < kSampleSize / 2; i++) {
MoveTo(&sim, &estimator, 3.5 * index_diff);
@@ -163,8 +164,8 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
MoveTo(&sim, &estimator, 3.1 * index_diff);
@@ -179,8 +180,8 @@
double index_diff = 0.6;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
for (unsigned int i = 0; i < kSampleSize; i++) {
@@ -214,8 +215,8 @@
const double known_index_pos = 3.5 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.3 * index_diff, index_diff / 3.0, known_index_pos);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, known_index_pos});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
for (unsigned int i = 0; i < kSampleSize; i++) {
@@ -239,8 +240,8 @@
TEST_F(ZeroingTest, BasicErrorAPITest) {
const double index_diff = 1.0;
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
PositionSensorSimulator sim(index_diff);
sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
@@ -260,5 +261,37 @@
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) {
+ const double index_diff = 0.8;
+ const double known_index_pos = 2 * index_diff;
+ int sample_size = 30;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ sample_size, index_diff, known_index_pos, kIndexErrorFraction});
+
+ for (int i = 0; i < sample_size; i++) {
+ MoveTo(&sim, &estimator, 13 * index_diff);
+ }
+ MoveTo(&sim, &estimator, 8 * index_diff);
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+ sim.Initialize(9.0 * index_diff + 0.31 * index_diff, index_diff / 3.0,
+ known_index_pos);
+ MoveTo(&sim, &estimator, 9 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.error());
+}
+
} // namespace zeroing
} // namespace frc971