Write class to handle gyro zeroing
Since we're moving the gyro zeroing into the drivetrain, take the
opportunity to write a new class to wrap it and to handle automatically
zeroing us any time we stay still for 5 seconds.
Change-Id: I9be7c970b6bbe3cf1eddc217c93467dfc21cd4cd
diff --git a/frc971/zeroing/averager_test.cc b/frc971/zeroing/averager_test.cc
index 930a3cb..9f8f727 100644
--- a/frc971/zeroing/averager_test.cc
+++ b/frc971/zeroing/averager_test.cc
@@ -17,7 +17,7 @@
averager.AddData(static_cast<int>(i));
}
ASSERT_TRUE(averager.full());
- ASSERT_EQ(2, averager.GetAverage());
+ ASSERT_EQ(2, averager.GetAverage()(0, 0));
}
// Makes sure that we can compute the average of a bunch of floats.
@@ -28,7 +28,32 @@
averager.AddData(static_cast<float>(i) / 3.0);
}
ASSERT_TRUE(averager.full());
- ASSERT_NEAR(16.5, averager.GetAverage(), 0.001);
+ ASSERT_NEAR(16.5, averager.GetAverage()(0, 0), 0.001);
+}
+
+TEST_F(AveragerTest, CalculateRange) {
+ Averager<float, 5, 2> averager;
+ ASSERT_EQ(0, averager.GetRange());
+ averager.AddData({100, 10});
+ averager.AddData({105, 15});
+ averager.AddData({90, 9});
+ ASSERT_EQ(15, averager.GetRange());
+ for (size_t ii = 0; ii < averager.size(); ++ii) {
+ averager.AddData({10, 20});
+ }
+ ASSERT_EQ(0, averager.GetRange());
+}
+
+TEST_F(AveragerTest, ResetAverager) {
+ Averager<float, 5> averager;
+ for (size_t ii = 0; ii < averager.size(); ++ii) {
+ averager.AddData(10);
+ }
+ ASSERT_TRUE(averager.full());
+ ASSERT_EQ(10.0, averager.GetAverage()(0, 0));
+ averager.Reset();
+ ASSERT_FALSE(averager.full());
+ ASSERT_EQ(0.0, averager.GetAverage()(0, 0));
}
} // namespace zeroing