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/y2020/control_loops/superstructure/hood/BUILD b/y2020/control_loops/superstructure/hood/BUILD
index d0e5953..3035a55 100644
--- a/y2020/control_loops/superstructure/hood/BUILD
+++ b/y2020/control_loops/superstructure/hood/BUILD
@@ -32,3 +32,18 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "hood_encoder_zeroing_estimator",
+ srcs = [
+ "hood_encoder_zeroing_estimator.cc",
+ ],
+ hdrs = [
+ "hood_encoder_zeroing_estimator.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971/zeroing:zeroing",
+ "//y2020:constants",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
new file mode 100644
index 0000000..eb9351b
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
@@ -0,0 +1,42 @@
+#include <cmath>
+
+#include "y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h"
+
+namespace y2020::control_loops::superstructure::hood {
+
+HoodEncoderZeroingEstimator::HoodEncoderZeroingEstimator(
+ const frc971::constants::AbsoluteAndAbsoluteEncoderZeroingConstants
+ &constants)
+ : AbsoluteAndAbsoluteEncoderZeroingEstimator(constants),
+ hood_geometry_(constants::GetValues().hood_geometry) {
+ constants::InitValues();
+}
+
+double HoodEncoderZeroingEstimator::AdjustedSingleTurnAbsoluteEncoder(
+ const AbsoluteAndAbsoluteEncoderZeroingEstimator::PositionStruct &sample)
+ const {
+ // Using equation derived in
+ // https://slack-files.com/T2752T5SA-F02JJ39RY03-f83f22b78d
+ // In that equation, theta = theta, l = screw_length, b = diagonal_length,
+ // a = back_plate_diagonal_length, and r = radius.
+ const double theta =
+ frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator::
+ AdjustedSingleTurnAbsoluteEncoder(sample) +
+ hood_geometry_.theta_0;
+
+ const double screw_length =
+ std::sqrt(std::pow(hood_geometry_.radius, 2) +
+ std::pow(hood_geometry_.diagonal_length, 2) -
+ (2 * hood_geometry_.radius * hood_geometry_.diagonal_length *
+ std::cos(theta)) -
+ std::pow(hood_geometry_.back_plate_diagonal_length, 2)) -
+ hood_geometry_.screw_length_0;
+ constexpr double kMToIn = 39.3701;
+ const double adjusted_single_turn_absolute_encoder =
+ (screw_length * kMToIn) *
+ constants::Values::kHoodEncoderRadiansPerInTravel();
+
+ return adjusted_single_turn_absolute_encoder;
+}
+
+} // namespace y2020::control_loops::superstructure::hood
diff --git a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h
new file mode 100644
index 0000000..238154b
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h
@@ -0,0 +1,24 @@
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "y2020/constants.h"
+
+namespace y2020::control_loops::superstructure::hood {
+
+using AbsoluteAndAbsoluteEncoderZeroingEstimator =
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator;
+
+class HoodEncoderZeroingEstimator
+ : public AbsoluteAndAbsoluteEncoderZeroingEstimator {
+ public:
+ HoodEncoderZeroingEstimator(
+ const frc971::constants::AbsoluteAndAbsoluteEncoderZeroingConstants
+ &constants);
+
+ private:
+ double AdjustedSingleTurnAbsoluteEncoder(
+ const AbsoluteAndAbsoluteEncoderZeroingEstimator::PositionStruct &sample)
+ const override;
+
+ const constants::Values::HoodGeometry hood_geometry_;
+};
+
+} // namespace y2020::control_loops::superstructure::hood