Added Zeroing Estimator for the column
Change-Id: I0c9dc557d91ed62f48d8ab6a9a20d2508e362b82
diff --git a/frc971/constants.h b/frc971/constants.h
index 7b4cb47..0171e47 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -6,6 +6,21 @@
namespace frc971 {
namespace constants {
+struct HallEffectZeroingConstants {
+ // The absolute position of the lower edge of the hall effect sensor.
+ double lower_hall_position;
+ // The absolute position of the upper edge of the hall effect sensor.
+ double upper_hall_position;
+ // The difference in scaled units between two hall effect edges. This is the
+ // number of units/cycle.
+ double index_difference;
+ // Number of cycles we need to see the hall effect high.
+ size_t hall_trigger_zeroing_length;
+ // Direction the system must be moving in order to zero. True is positive,
+ // False is negative direction.
+ bool zeroing_move_direction;
+};
+
struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 757e257..0f248f1 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -98,6 +98,20 @@
int32_t index_pulses_seen;
};
+struct HallEffectAndPositionEstimatorState {
+ // If error.
+ bool error;
+ // If we've found a positive edge while moving backwards and is zeroed.
+ bool zeroed;
+ // Encoder angle relative to where we started.
+ double encoder;
+ // The positions of the extreme posedges we've seen.
+ // If we've gotten enough samples where the hall effect is high before can be
+ // certain it is not a false positive.
+ bool high_long_enough;
+ double offset;
+};
+
// A left/right pair of PotAndIndexPositions.
struct PotAndIndexPair {
PotAndIndexPosition left;
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index b8addc1..0f09f7f 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -370,6 +370,18 @@
EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
EXPECT_EQ(4, position.negedge_count);
EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+
+ for (int i = 0; i < 10; ++i) {
+ // Now, go over the lower edge, falling.
+ sim.MoveTo(-0.25 - i * 1.0e-6);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_NEAR(-i * 1.0e-6, position.position, 1e-8);
+ EXPECT_EQ(4, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
+ EXPECT_EQ(4, position.negedge_count);
+ EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ }
}
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 947a15c..4fe0236 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -142,6 +142,118 @@
return r;
}
+HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants)
+ : constants_(constants) {
+ Reset();
+}
+
+void HallEffectAndPositionZeroingEstimator::Reset() {
+ offset_ = 0.0;
+ min_low_position_ = ::std::numeric_limits<double>::max();
+ max_low_position_ = ::std::numeric_limits<double>::lowest();
+ zeroed_ = false;
+ initialized_ = false;
+ last_used_posedge_count_ = 0;
+ cycles_high_ = 0;
+ high_long_enough_ = false;
+ first_start_pos_ = 0.0;
+ error_ = false;
+ current_ = 0.0;
+ first_start_pos_ = 0.0;
+}
+
+void HallEffectAndPositionZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
+}
+
+void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
+ const HallEffectAndPosition &info) {
+ // If we have a new posedge.
+ if (!info.current) {
+ if (last_hall_) {
+ min_low_position_ = max_low_position_ = info.position;
+ } else {
+ min_low_position_ = ::std::min(min_low_position_, info.position);
+ max_low_position_ = ::std::max(max_low_position_, info.position);
+ }
+ }
+ last_hall_ = info.current;
+}
+
+void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
+ const HallEffectAndPosition &info) {
+ // We want to make sure that we encounter at least one posedge while zeroing.
+ // So we take the posedge count from the first sample after reset and wait for
+ // that count to change and for the hall effect to stay high before we
+ // consider ourselves zeroed.
+ if (!initialized_) {
+ last_used_posedge_count_ = info.posedge_count;
+ initialized_ = true;
+ last_hall_ = info.current;
+ }
+
+ StoreEncoderMaxAndMin(info);
+
+ if (info.current) {
+ cycles_high_++;
+ } else {
+ cycles_high_ = 0;
+ last_used_posedge_count_ = info.posedge_count;
+ }
+
+ high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
+
+ bool moving_backward = false;
+ if (constants_.zeroing_move_direction) {
+ moving_backward = info.position > min_low_position_;
+ } else {
+ moving_backward = info.position < max_low_position_;
+ }
+
+ // If there are no posedges to use or we don't have enough samples yet to
+ // have a well-filtered starting position then we use the filtered value as
+ // our best guess.
+ if (last_used_posedge_count_ != info.posedge_count && high_long_enough_ &&
+ moving_backward) {
+ // Note the offset and the current posedge count so that we only run this
+ // logic once per posedge. That should be more resilient to corrupted
+ // intermediate data.
+ offset_ = -info.posedge_value;
+ if (constants_.zeroing_move_direction) {
+ offset_ += constants_.lower_hall_position;
+ } else {
+ offset_ += constants_.upper_hall_position;
+ }
+ last_used_posedge_count_ = info.posedge_count;
+
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = offset_;
+ 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;
+ }
+
+ position_ = info.position - offset_;
+}
+
+HallEffectAndPositionZeroingEstimator::State
+HallEffectAndPositionZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.encoder = position_;
+ r.high_long_enough = high_long_enough_;
+ r.offset = offset_;
+ return r;
+}
PotAndAbsEncoderZeroingEstimator::PotAndAbsEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 7b77fdd..91d5b8c 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -121,6 +121,80 @@
double first_start_pos_;
};
+// Estimates the position with an incremental encoder with an index pulse and a
+// potentiometer.
+class HallEffectAndPositionZeroingEstimator : public ZeroingEstimator {
+ public:
+ using Position = HallEffectAndPosition;
+ using ZeroingConstants = constants::HallEffectZeroingConstants;
+ using State = HallEffectAndPositionEstimatorState;
+
+ explicit HallEffectAndPositionZeroingEstimator(const ZeroingConstants &constants);
+
+ // Update the internal logic with the next sensor values.
+ void UpdateEstimate(const Position &info);
+
+ // Reset the internal logic so it needs to be re-zeroed.
+ void Reset();
+
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError();
+
+ bool error() const override { return error_; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
+ private:
+ // Sets the minimum and maximum posedge position values.
+ void StoreEncoderMaxAndMin(const HallEffectAndPosition &info);
+
+ // The zeroing constants used to describe the configuration of the system.
+ const ZeroingConstants constants_;
+
+ // The estimated state of the hall effect.
+ double current_ = 0.0;
+ // The estimated position.
+ double position_ = 0.0;
+ // The smallest and largest positions of the last set of encoder positions
+ // while the hall effect was low.
+ double min_low_position_;
+ double max_low_position_;
+ // If we've seen the hall effect high for enough times without going low, then
+ // we can be sure it isn't a false positive.
+ bool high_long_enough_;
+ size_t cycles_high_;
+
+ bool last_hall_ = false;
+
+ // The estimated starting position of the mechanism. We also call this the
+ // 'offset' in some contexts.
+ double offset_;
+ // Flag for triggering logic that takes note of the current posedge count
+ // after a reset. See `last_used_posedge_count_'.
+ bool initialized_;
+ // After a reset we keep track of the posedge count with this. Only after the
+ // posedge count changes (i.e. increments at least once or wraps around) will
+ // we consider the mechanism zeroed. We also use this to store the most recent
+ // `HallEffectAndPosition::posedge_count' value when the start position
+ // was calculated. It helps us calculate the start position only on posedges
+ // to reject corrupted intermediate data.
+ int32_t last_used_posedge_count_;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_ = false;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+};
+
// Estimates the position with an absolute encoder which also reports
// incremental counts, and a potentiometer.
class PotAndAbsEncoderZeroingEstimator : public ZeroingEstimator {
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 32ca049..a4ba3d7 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -55,6 +55,15 @@
estimator->UpdateEstimate(sensor_values_);
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ HallEffectAndPositionZeroingEstimator *estimator,
+ double new_position) {
+ HallEffectAndPosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
+
::aos::testing::TestSharedMemory my_shm_;
};
@@ -438,5 +447,88 @@
estimator.GetEstimatorState().position);
}
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(ZeroingTest, TestHallEffectZeroing) {
+ constants::HallEffectZeroingConstants constants;
+ constants.lower_hall_position = 0.25;
+ constants.upper_hall_position = 0.75;
+ constants.index_difference = 1.0;
+ constants.hall_trigger_zeroing_length = 2;
+ constants.zeroing_move_direction = false;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos = 1.0;
+
+ sim.InitializeHallEffectAndPosition(start_pos, constants.lower_hall_position,
+ constants.upper_hall_position);
+
+ HallEffectAndPositionZeroingEstimator estimator(constants);
+
+ // Should not be zeroed when we stand still.
+ for (int i = 0; i < 300; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, 0.9);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move to where the hall effect is triggered and make sure it becomes zeroed.
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ ASSERT_TRUE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly.
+ EXPECT_DOUBLE_EQ(-0.25, estimator.offset());
+
+ // Make sure triggering errors works.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Ensure resetting resets the state of the estimator.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+
+ // Make sure we don't become zeroed if the hall effect doesn't trigger for
+ // long enough.
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Make sure we can zero moving in the opposite direction as before and stay
+ // zeroed once the hall effect is no longer triggered.
+
+ MoveTo(&sim, &estimator, 0.0);
+ ASSERT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.4);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.6);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly.
+ EXPECT_DOUBLE_EQ(-0.75, estimator.offset());
+
+ // Make sure we don't zero if we start in the hall effect's range, before we
+ // reset, we also check that there were no errors.
+ MoveTo(&sim, &estimator, 0.5);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+ estimator.Reset();
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+}
+
+
} // namespace zeroing
} // namespace frc971