commit 051c700a1d7ca8076ca0b94dec94e906d1b8f7b4
author milind-u <milind.upadhyay@gmail.com> Mon Feb 20 16:28:18 2023 -0800
committer milind-u <milind.upadhyay@gmail.com> Mon Feb 20 21:24:17 2023 -0800
tree c37b60de34d74372fd31c3b4c02026ce19818bfe
parent f8bd1773b93d7356083700b2b569788749681c91

Add y2023 wrist and roll constants

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I801842f0852a0e93c10b16cd83621de3b05b039c

y2023/constants.h

diff --git a/y2023/constants.h b/y2023/constants.h
index 64153af..f8e766c 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -10,6 +10,8 @@
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
 #include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/roll/roll_plant.h"
+#include "y2023/control_loops/superstructure/wrist/wrist_plant.h"
 
 namespace y2023 {
 namespace constants {
@@ -65,17 +67,78 @@
     return (24.0 / 36.0) * (24.0 / 58.0) * (15.0 / 95.0);
   }
 
+  static constexpr double kProximalPotRadiansPerVolt() {
+    return kProximalPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+           (2 * M_PI /*radians*/);
+  }
+
   static constexpr double kDistalEncoderCountsPerRevolution() { return 4096.0; }
   static constexpr double kDistalEncoderRatio() { return (15.0 / 95.0); }
   static constexpr double kMaxDistalEncoderPulsesPerSecond() {
     return control_loops::superstructure::arm::kArmConstants.free_speed /
            (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
-           kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
+           kDistalEncoderRatio() * kDistalEncoderCountsPerRevolution();
   }
   static constexpr double kDistalPotRatio() {
     return (24.0 / 36.0) * (18.0 / 66.0) * (15.0 / 95.0);
   }
 
+  static constexpr double kDistalPotRadiansPerVolt() {
+    return kDistalPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+           (2 * M_PI /*radians*/);
+  }
+
+  // Roll joint
+  static constexpr double kRollJointEncoderCountsPerRevolution() {
+    return 4096.0;
+  }
+
+  static constexpr double kRollJointEncoderRatio() { return (18.0 / 48.0); }
+
+  static constexpr double kRollJointPotRatio() { return (18.0 / 48.0); }
+
+  static constexpr double kRollJointPotRadiansPerVolt() {
+    return kRollJointPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+           (2 * M_PI /*radians*/);
+  }
+
+  static constexpr double kMaxRollJointEncoderPulsesPerSecond() {
+    return control_loops::superstructure::roll::kFreeSpeed / (2.0 * M_PI) *
+           control_loops::superstructure::roll::kOutputRatio /
+           kRollJointEncoderRatio() * kRollJointEncoderCountsPerRevolution();
+  }
+
+  static constexpr ::frc971::constants::Range kRollJointRange() {
+    return ::frc971::constants::Range{
+        -1.05,  // Back Hard
+        1.44,   // Front Hard
+        -0.89,  // Back Soft
+        1.26    // Front Soft
+    };
+  }
+
+  // Wrist
+  static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
+
+  static constexpr double kWristEncoderRatio() {
+    return (24.0 / 36.0) * (36.0 / 60.0);
+  }
+
+  static constexpr double kMaxWristEncoderPulsesPerSecond() {
+    return control_loops::superstructure::wrist::kFreeSpeed / (2.0 * M_PI) *
+           control_loops::superstructure::wrist::kOutputRatio /
+           kWristEncoderRatio() * kWristEncoderCountsPerRevolution();
+  }
+
+  static constexpr ::frc971::constants::Range kWristRange() {
+    return ::frc971::constants::Range{
+        -1.05,  // Back Hard
+        1.44,   // Front Hard
+        -0.89,  // Back Soft
+        1.26    // Front Soft
+    };
+  }
+
   struct PotConstants {
     ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
         ::frc971::zeroing::RelativeEncoderZeroingEstimator>
@@ -97,6 +160,12 @@
 
   ArmJointConstants arm_proximal;
   ArmJointConstants arm_distal;
+
+  PotAndAbsEncoderConstants roll_joint;
+
+  ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+      ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+      wrist;
 };
 
 // Creates and returns a Values instance for the constants.