Remove position() member from zeroing estimators
It didn't return anything particularly useful, so it should not have
been used anywhere. Turns out it was only used in the test, so it was
pretty easy.
Change-Id: I3f81bd16a1a8e429c9748abd4e9dc9c7e0259dc5
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index f3d224b..cb95df8 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -77,6 +77,23 @@
double pot_position;
};
+// The internal state of a zeroing estimator.
+struct IndexEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ bool error;
+ // If the joint has seen an index pulse and is zeroed.
+ bool zeroed;
+ // The estimated position of the joint. This is just the position relative to
+ // where we started if we're not zeroed yet.
+ double position;
+
+ // The positions of the extreme index pulses we've seen.
+ double min_index_position;
+ double max_index_position;
+ // The number of index pulses we've seen.
+ int32_t index_pulses_seen;
+};
+
// A left/right pair of PotAndIndexPositions.
struct PotAndIndexPair {
PotAndIndexPosition left;
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 70fbb56..d22e48a 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -336,7 +336,7 @@
}
}
-int PulseIndexZeroingEstimator::IndexPulseCount() {
+int PulseIndexZeroingEstimator::IndexPulseCount() const {
if (min_index_position_ > max_index_position_) {
// This condition means we haven't seen a pulse yet.
return 0;
@@ -364,9 +364,20 @@
min_index_position_;
zeroed_ = true;
}
- if (zeroed_) {
- position_ = info.encoder + offset_;
- }
+
+ position_ = info.encoder + offset_;
+}
+
+PulseIndexZeroingEstimator::State
+PulseIndexZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.position = position_;
+ r.min_index_position = min_index_position_;
+ r.max_index_position = max_index_position_;
+ r.index_pulses_seen = IndexPulseCount();
+ return r;
}
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 53b8e31..4b00c23 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -30,9 +30,6 @@
// Returns the estimated position of the corresponding mechanism.
virtual double offset() const = 0;
- // Returns the estimated starting position of the corresponding mechansim.
- virtual double position() const = 0;
-
// Returns true if there has been an error.
virtual bool error() const = 0;
};
@@ -62,8 +59,6 @@
bool zeroed() const override { return zeroed_; }
- double position() const override { return position_; }
-
double offset() const override { return offset_; }
// Returns a number between 0 and 1 that represents the percentage of the
@@ -145,8 +140,6 @@
bool zeroed() const override { return zeroed_; }
- double position() const override { return position_; }
-
double offset() const override { return offset_; }
bool error() const override { return error_; }
@@ -198,7 +191,7 @@
public:
using Position = IndexPosition;
using ZeroingConstants = constants::PotAndIndexPulseZeroingConstants;
- using State = EstimatorState;
+ using State = IndexEstimatorState;
PulseIndexZeroingEstimator(
const constants::EncoderPlusIndexZeroingConstants &constants)
@@ -211,11 +204,6 @@
bool zeroed() const override { return zeroed_; }
- double position() const override {
- CHECK(zeroed_);
- return position_;
- }
-
double offset() const override { return offset_; }
bool error() const override { return error_; }
@@ -223,6 +211,9 @@
// Updates the internal logic with the next sensor values.
void UpdateEstimate(const IndexPosition &info);
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
private:
// Returns the current real position using the relative encoder offset.
double CalculateCurrentPosition(const IndexPosition &info);
@@ -231,7 +222,7 @@
void StoreIndexPulseMaxAndMin(const IndexPosition &info);
// Returns the number of index pulses we should have seen so far.
- int IndexPulseCount();
+ int IndexPulseCount() const;
// Contains the physical constants describing the system.
const constants::EncoderPlusIndexZeroingConstants constants_;
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 6373056..e5c6813 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -71,13 +71,13 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.3 * index_diff);
}
- ASSERT_NEAR(3.3 * index_diff, estimator.position(),
+ ASSERT_NEAR(3.3 * index_diff, estimator.GetEstimatorState().position,
kAcceptableUnzeroedError * index_diff);
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.9 * index_diff);
}
- ASSERT_NEAR(3.9 * index_diff, estimator.position(),
+ ASSERT_NEAR(3.9 * index_diff, estimator.GetEstimatorState().position,
kAcceptableUnzeroedError * index_diff);
}
@@ -114,24 +114,25 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.6);
}
- ASSERT_NEAR(3.6, estimator.position(), kAcceptableUnzeroedError * index_diff);
+ ASSERT_NEAR(3.6, estimator.GetEstimatorState().position,
+ kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, estimator.position(), 0.001);
+ ASSERT_NEAR(4.01, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, estimator.position(), 0.001);
+ ASSERT_NEAR(4.99, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, estimator.position(), 0.001);
+ ASSERT_NEAR(3.99, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, estimator.position(), 0.001);
+ ASSERT_NEAR(3.01, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, estimator.position(), 0.001);
+ ASSERT_NEAR(13.55, estimator.GetEstimatorState().position, 0.001);
}
TEST_F(ZeroingTest, TestDifferentIndexDiffs) {
@@ -147,25 +148,25 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.5 * index_diff);
}
- ASSERT_NEAR(3.5 * index_diff, estimator.position(),
+ ASSERT_NEAR(3.5 * index_diff, estimator.GetEstimatorState().position,
kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, estimator.position(), 0.001);
+ ASSERT_NEAR(4.01, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, estimator.position(), 0.001);
+ ASSERT_NEAR(4.99, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, estimator.position(), 0.001);
+ ASSERT_NEAR(3.99, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, estimator.position(), 0.001);
+ ASSERT_NEAR(3.01, estimator.GetEstimatorState().position, 0.001);
MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, estimator.position(), 0.001);
+ ASSERT_NEAR(13.55, estimator.GetEstimatorState().position, 0.001);
}
TEST_F(ZeroingTest, TestPercentage) {
@@ -258,12 +259,12 @@
MoveTo(&sim, &estimator, 3.7 * index_diff);
ASSERT_TRUE(estimator.zeroed());
ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
- ASSERT_DOUBLE_EQ(3.7 * index_diff, estimator.position());
+ ASSERT_DOUBLE_EQ(3.7 * index_diff, estimator.GetEstimatorState().position);
// Trigger one more index pulse and check the offset.
MoveTo(&sim, &estimator, 4.7 * index_diff);
ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
- ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.position());
+ ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.GetEstimatorState().position);
}
TEST_F(ZeroingTest, BasicErrorAPITest) {
@@ -439,10 +440,12 @@
ASSERT_TRUE(estimator.zeroed());
ASSERT_DOUBLE_EQ(start_pos, estimator.offset());
- ASSERT_DOUBLE_EQ(3.5 * constants.index_difference, estimator.position());
+ ASSERT_DOUBLE_EQ(3.5 * constants.index_difference,
+ estimator.GetEstimatorState().position);
MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
- ASSERT_DOUBLE_EQ(0.5 * constants.index_difference, estimator.position());
+ ASSERT_DOUBLE_EQ(0.5 * constants.index_difference,
+ estimator.GetEstimatorState().position);
}
} // namespace zeroing