No zeroing while moving for absolute encoders
Modified zeroing code for pot and absolute encoder to no longer
update estimates of offsets while moving. This causes a delay of
3 samples (due to a sample buffer size).
Change-Id: Iaf82ad83ebcfc68fa75ce23716c7d89a6e34a917
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 4d36ca9..8c730db 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -173,8 +173,15 @@
::std::vector<double> relative_to_absolute_offset_samples_;
// Offset between the Pot and Relative encoder position.
::std::vector<double> offset_samples_;
+ // Last moving_buffer_size position samples to be used to determine if the
+ // robot is moving.
+ ::std::vector<PotAndAbsolutePosition> buffered_samples_;
+ // Pointer to front of the buffered samples.
+ int buffered_samples_idx_ = 0;
+ // Estimated offset between the pot and relative encoder.
+ double pot_relative_encoder_offset_ = 0;
// Estimated start position of the mechanism
- double offset_;
+ double offset_ = 0;
// The next position in 'relative_to_absolute_offset_samples_' and
// 'encoder_samples_' to be used to store the next sample.
int samples_idx_;