Correct hood zeroing calculations
Used trig to find screw length based on hood angle, preventing incorrect
estops. Calculations based on
https://slack-files.com/T2752T5SA-F02JJ39RY03-f83f22b78d
Change-Id: I8a143d6dfe6476a4a57b702f9bd93cad5c6b9262
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.cc b/frc971/zeroing/absolute_and_absolute_encoder.cc
index 0645d43..40b0519 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder.cc
@@ -35,6 +35,15 @@
error_ = false;
}
+double
+AbsoluteAndAbsoluteEncoderZeroingEstimator::AdjustedSingleTurnAbsoluteEncoder(
+ const PositionStruct &sample) const {
+ return UnWrap(constants_.single_turn_middle_position,
+ sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position,
+ constants_.single_turn_one_revolution_distance);
+}
+
// So, this needs to be a multistep process. We need to first estimate the
// offset between the absolute encoder and the relative encoder. That process
// should get us an absolute number which is off by integer multiples of the
@@ -137,10 +146,7 @@
}
const double adjusted_single_turn_absolute_encoder =
- UnWrap(constants_.single_turn_middle_position,
- sample.single_turn_absolute_encoder -
- constants_.single_turn_measured_absolute_position,
- constants_.single_turn_one_revolution_distance);
+ AdjustedSingleTurnAbsoluteEncoder(sample);
// Now compute the offset between the pot and relative encoder.
if (offset_samples_.size() < constants_.average_filter_size) {