Added filtered absolute encoder.
Change-Id: I2b22f251b4585e3cc93d0ad10a1cf7e80dc6731b
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index cb95df8..4d87587 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -75,6 +75,10 @@
// The estimated position not using the index pulse.
double pot_position;
+
+ // The estimated absolute position of the encoder. This is filtered, so it
+ // can be easily used when zeroing.
+ double absolute_position;
};
// The internal state of a zeroing estimator.
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 2627d7f..947a15c 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -230,15 +230,29 @@
: relative_to_absolute_offset_sum /
relative_to_absolute_offset_samples_.size();
+ const double adjusted_incremental_encoder =
+ buffered_samples_[middle_index].encoder +
+ average_relative_to_absolute_offset;
+
// Now, compute the nearest absolute encoder value to the offset relative
// encoder position.
const double adjusted_absolute_encoder =
- Wrap(buffered_samples_[middle_index].encoder +
- average_relative_to_absolute_offset,
+ Wrap(adjusted_incremental_encoder,
buffered_samples_[middle_index].absolute_encoder -
constants_.measured_absolute_position,
constants_.one_revolution_distance);
+ // Reverse the math on the previous line to compute the absolute encoder.
+ // Do this by taking the adjusted encoder, and then subtracting off the
+ // second argument above, and the value that was added by Wrap.
+ filtered_absolute_encoder_ =
+ ((buffered_samples_[middle_index].encoder +
+ average_relative_to_absolute_offset) -
+ (-constants_.measured_absolute_position +
+ (adjusted_absolute_encoder -
+ (buffered_samples_[middle_index].absolute_encoder -
+ constants_.measured_absolute_position))));
+
const double relative_to_absolute_offset =
adjusted_absolute_encoder - buffered_samples_[middle_index].encoder;
@@ -323,6 +337,7 @@
r.zeroed = zeroed_;
r.position = position_;
r.pot_position = filtered_position_;
+ r.absolute_position = filtered_absolute_encoder_;
return r;
}
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index ac2e49e..4b21bc4 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -166,6 +166,9 @@
// changes to true.
double first_offset_;
+ // The filtered absolute encoder. This is used in the status for calibration.
+ double filtered_absolute_encoder_ = 0.0;
+
// Samples of the offset needed to line the relative encoder up with the
// absolute encoder.
::std::vector<double> relative_to_absolute_offset_samples_;