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>

y2020/constants.cc

diff --git a/y2020/constants.cc b/y2020/constants.cc
index 1ef9073..80448e8 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -28,9 +28,10 @@
   ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
       ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
       *const hood = &r->hood;
+  Values::HoodGeometry *const hood_geometry = &r->hood_geometry;
 
-  // We found that the finisher velocity does not change ball velocity much, so
-  // keep it constant.
+  // We found that the finisher velocity does not change ball velocity much,
+  // so keep it constant.
   constexpr double kVelocityFinisher = 350.0;
   r->shot_interpolation_table =
       InterpolationTable<Values::ShotParams>({{1.0, {0.01, 10.7}},
@@ -80,6 +81,14 @@
   hood->zeroing_constants.measured_absolute_position = 0;
   hood->zeroing_constants.single_turn_measured_absolute_position = 0;
 
+  constexpr double kDegToRad = M_PI / 180.0;
+  constexpr double kMmToM = 1.0 / 1000.0;
+  hood_geometry->theta_0 = 22.98004 * kDegToRad;
+  hood_geometry->screw_length_0 = 110.33888 * kMmToM;
+  hood_geometry->radius = 269.6262 * kMmToM;
+  hood_geometry->diagonal_length = 288.4353 * kMmToM;
+  hood_geometry->back_plate_diagonal_length = 22.86 * kMmToM;
+
   ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
       ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
       &r->intake;