Add relative encoder zeroing strategy
Change-Id: I07b5461a9766aa25c9c858b7771af26ed08ad4ff
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 61d2d0c..2b715ea 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -1,14 +1,14 @@
+#include "frc971/zeroing/zeroing.h"
+
#include <unistd.h>
#include <memory>
-
#include <random>
-#include "gtest/gtest.h"
-#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/zeroing/zeroing.h"
#include "aos/die.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
@@ -72,6 +72,14 @@
*simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ RelativeEncoderZeroingEstimator *estimator, double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<RelativePosition>(&fbb));
+ }
+
template <typename T>
double GetEstimatorPosition(T *estimator) {
FBB fbb;
@@ -789,5 +797,21 @@
ASSERT_TRUE(estimator.error());
}
+TEST_F(ZeroingTest, TestRelativeEncoderZeroingWithoutMovement) {
+ PositionSensorSimulator sim(1.0);
+ RelativeEncoderZeroingEstimator estimator;
+
+ sim.InitializeRelativeEncoder();
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.offset_ready());
+ EXPECT_DOUBLE_EQ(estimator.offset(), 0.0);
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.0);
+
+ MoveTo(&sim, &estimator, 0.1);
+
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.1);
+}
+
} // namespace zeroing
} // namespace frc971