Added Zeroing Estimator for the column
Change-Id: I0c9dc557d91ed62f48d8ab6a9a20d2508e362b82
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);
+ }
}