commit d53408ee271236d4b3ac2879b31b7eb833426481
author milind-u <milind.upadhyay@gmail.com> Thu Oct 21 19:43:58 2021 -0700
committer milind-u <milind.upadhyay@gmail.com> Wed Oct 27 20:09:36 2021 -0700
tree 351cdd5b10c26b1faf19f715a8afe09546522984
parent 84b08bec8f87138e0f6812082c281cd29f5c2d3f

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>

frc971/zeroing/absolute_and_absolute_encoder.cc

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) {

frc971/zeroing/absolute_and_absolute_encoder.h

diff --git a/frc971/zeroing/absolute_and_absolute_encoder.h b/frc971/zeroing/absolute_and_absolute_encoder.h
index dd2190d..499f7d1 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.h
+++ b/frc971/zeroing/absolute_and_absolute_encoder.h
@@ -48,7 +48,7 @@
   virtual flatbuffers::Offset<State> GetEstimatorState(
       flatbuffers::FlatBufferBuilder *fbb) const override;
 
- private:
+ protected:
   struct PositionStruct {
     PositionStruct(const AbsoluteAndAbsolutePosition &position_buffer)
         : single_turn_absolute_encoder(
@@ -60,6 +60,12 @@
     double encoder;
   };
 
+  // Returns an adjusted single turn absolute encoder reading.
+  // Filled in by default but can be overriden.
+  virtual double AdjustedSingleTurnAbsoluteEncoder(
+      const PositionStruct &sample) const;
+
+ private:
   // The zeroing constants used to describe the configuration of the system.
   const constants::AbsoluteAndAbsoluteEncoderZeroingConstants constants_;