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.h b/frc971/zeroing/averager.h
index 879c246..b6ff09d 100644
--- a/frc971/zeroing/averager.h
+++ b/frc971/zeroing/averager.h
@@ -5,45 +5,79 @@
#include <array>
#include <stdint.h>
+#include "Eigen/Dense"
+#include "glog/logging.h"
+
namespace frc971 {
namespace zeroing {
// Averages a set of given numbers. Numbers are given one at a time. Once full
// the average may be requested.
-template <typename data_type, size_t data_size>
+// TODO(james): Look at deduplicating this with some of the work in the
+// MoveDetector.
+template <typename Scalar, size_t num_samples, int rows_per_sample = 1>
class Averager {
public:
+ typedef Eigen::Matrix<Scalar, rows_per_sample, 1> Vector;
// Adds one data point to the set of data points to be averaged.
- // If more than "data_size" samples are added, they will start overwriting
+ // If more than "num_samples" samples are added, they will start overwriting
// the oldest ones.
- void AddData(data_type data) {
+ void AddData(Scalar data) {
+ CHECK_EQ(1, rows_per_sample);
+ AddData(Vector(data));
+ }
+ void AddData(const Vector &data) {
data_[data_point_index_] = data;
- num_data_points_ = ::std::min(data_size, num_data_points_ + 1);
- data_point_index_ = (data_point_index_ + 1) % data_size;
+ num_data_points_ = std::min(num_samples, num_data_points_ + 1);
+ data_point_index_ = (data_point_index_ + 1) % num_samples;
}
// Returns the average of the data points.
- data_type GetAverage() const {
- // TODO(phil): What do we want to do without any elements?
+ Vector GetAverage() const {
if (num_data_points_ == 0) {
- return 0;
+ return Vector::Zero();
}
- data_type average = 0;
- for (data_type data : data_) {
+ Vector average;
+ average.setZero();
+ for (const Vector &data : data_) {
average += data;
}
return average / num_data_points_;
}
- // Returns true when we've gathered data_size data points.
- bool full() const { return num_data_points_ >= data_size; };
+ // Return the difference between the min and max values in the data buffer.
+ Scalar GetRange() const {
+ if (num_data_points_ == 0) {
+ return 0.0;
+ }
+ Vector min_value;
+ min_value.setConstant(std::numeric_limits<Scalar>::max());
+ Vector max_value;
+ max_value.setConstant(std::numeric_limits<Scalar>::lowest());
+ // The array will always fill up starting at zero, so we can iterate from
+ // zero safely.
+ for (size_t ii = 0; ii < num_data_points_; ++ii) {
+ const Vector &value = data_[ii];
+ min_value = min_value.cwiseMin(value);
+ max_value = max_value.cwiseMax(value);
+ }
+ return (max_value - min_value).maxCoeff();
+ }
- size_t size() const { return data_size; }
+ void Reset() {
+ num_data_points_ = 0;
+ data_point_index_ = 0;
+ }
+
+ // Returns true when we've gathered num_samples data points.
+ bool full() const { return num_data_points_ >= num_samples; };
+
+ size_t size() const { return num_samples; }
private:
// Data points to be averaged.
- ::std::array<data_type, data_size> data_;
+ std::array<Vector, num_samples> data_;
// Which data point in "data_" will be filled in next.
size_t data_point_index_ = 0;
// Number of data points added via AddData().