refactor(arm): Use units for variables.

src/main/java/frc/lib/JointConstants.java

@@ -4,30 +4,30 @@
 import java.util.Objects;
 
 public record JointConstants(
-    double mass,
-    double length,
-    double radius,
-    double moi,
+    double massKg,
+    double lengthMeters,
+    double radiusMeters,
+    double moiKgMetersSquared,
     double gearing,
     DCMotor motor,
     int motorCount) {
 
   /**
    * Creates joint constants for the joint of an arm.
    *
-   * @param mass the joint's mass in kilograms.
-   * @param length the joint's length in meters.
+   * @param massKg the joint's mass in kilograms.
+   * @param lengthMeters the joint's length in meters.
    * @param radius the distance between the joint's pivot and the joint's center of mass in meters.
-   * @param moi the joint's moment of inertia in kilograms meters squared.
+   * @param moiKgMetersSquared the joint's moment of inertia in kilograms meters squared.
    * @param gearing the gearing between the joint's motor and the joint's pivot.
    * @param motor the type of the joint's driving motor.
    * @param motorCount the number of the driving motors.
    */
   public JointConstants {
-    Objects.requireNonNull(mass);
-    Objects.requireNonNull(length);
-    Objects.requireNonNull(radius);
-    Objects.requireNonNull(moi);
+    Objects.requireNonNull(massKg);
+    Objects.requireNonNull(lengthMeters);
+    Objects.requireNonNull(radiusMeters);
+    Objects.requireNonNull(moiKgMetersSquared);
     Objects.requireNonNull(gearing);
     Objects.requireNonNull(motor);
     Objects.requireNonNull(motorCount);
@@ -36,23 +36,24 @@ public record JointConstants(
   /**
    * Creates joint constants for the joint of an arm.
    *
-   * @param mass the joint's mass in kilograms.
+   * @param massKg the joint's mass in kilograms.
    * @param length the joint's length in meters.
    * @param radius the distance between the joint's pivot and the joint's center of mass in meters.
    * @param moi the joint's moment of inertia in kilograms meters squared.
    * @param gearing the gearing between the joint's motor and the joint's pivot.
    * @param motor the type of the joint's driving motor.
    */
   public JointConstants(
-      double mass, double length, double radius, double moi, double gearing, DCMotor motor) {
-    this(mass, length, radius, moi, gearing, motor, 1);
+      double massKg, double length, double radius, double moi, double gearing, DCMotor motor) {
+    this(massKg, length, radius, moi, gearing, motor, 1);
   }
 
   /**
    * Calculates the torque generated by the joint's gearbox element for the feedforward matrix.
    *
    * @return the torque generated by the joint's gearbox element for the feedforward matrix.
    */
+  // TODO Determine units
   public double torque() {
     return gearing * motorCount * motor.KtNMPerAmp / motor.rOhms;
   }
@@ -62,6 +63,7 @@ public double torque() {
    *
    * @return the torque loss due to back-emf for the feedforward matrix.
    */
+  // TODO Determine units
   public double torqueLoss() {
     return gearing
         * gearing

src/main/java/frc/lib/TwoJointedArmFeedforward.java

@@ -23,13 +23,13 @@ public class TwoJointedArmFeedforward {
   private final SimpleMatrix B_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
 
   public TwoJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) {
-    this.m1 = shoulder.mass();
-    this.m2 = elbow.mass();
-    this.l1 = shoulder.length();
-    this.r1 = shoulder.radius();
-    this.r2 = elbow.radius();
-    this.I1 = shoulder.moi();
-    this.I2 = elbow.moi();
+    this.m1 = shoulder.massKg();
+    this.m2 = elbow.massKg();
+    this.l1 = shoulder.lengthMeters();
+    this.r1 = shoulder.radiusMeters();
+    this.r2 = elbow.radiusMeters();
+    this.I1 = shoulder.moiKgMetersSquared();
+    this.I2 = elbow.moiKgMetersSquared();
     this.g1 = shoulder.gearing();
     this.g2 = elbow.gearing();
 

src/main/java/frc/robot/RobotMechanisms.java

@@ -50,15 +50,15 @@ private void initializeArmMechanism() {
         armRoot.append(
             new MechanismLigament2d(
                 "shoulder",
-                ShoulderMotorConstants.JOINT_CONSTANTS.length(),
+                ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(),
                 0,
                 armThickness,
                 new Color8Bit(Color.kOrange)));
     elbow =
         shoulder.append(
             new MechanismLigament2d(
                 "elbow",
-                ElbowMotorConstants.JOINT_CONSTANTS.length(),
+                ElbowMotorConstants.JOINT_CONSTANTS.lengthMeters(),
                 0,
                 armThickness,
                 new Color8Bit(Color.kGreen)));

src/main/java/frc/robot/arm/ArmConstants.java

@@ -17,25 +17,16 @@ public static class ShoulderMotorConstants {
     /** Shoulder motor CAN. */
     public static final CAN CAN = new CAN(32);
 
-    /** Gearing between the soulder motor and the shoulder joint. */
-    private static final double GEARING = 51.2;
-
-    /** Moment of inertia of the shoulder, in kilograms meters squared. */
-    private static final double MOI = 0.07415;
-
-    /** Shoulder pivot to elbow pivot distance in meters. */
-    private static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775);
-
-    /** Mass of the shoulder joint in kilograms. */
-    private static final double MASS = Units.lbsToKilograms(3.154);
-
-    /** Distance between the shoulder joint and the shoulder joint's center of mass in meters. */
-    private static final double RADIUS = Units.inchesToMeters(8.962869);
-
     /** Joint constants for the shoulder joint. */
     public static final JointConstants JOINT_CONSTANTS =
         new JointConstants(
-            MASS, SHOULDER_TO_ELBOW_DISTANCE, RADIUS, MOI, GEARING, DCMotor.getNEO(1), 1);
+            Units.lbsToKilograms(3.154), // massKg
+            Units.inchesToMeters(16.775), // lengthMeters
+            Units.inchesToMeters(8.962869), // radiusMeters
+            0.07415,
+            51.2,
+            DCMotor.getNEO(1), // motor
+            1);
 
     /** Minimum angle of the shoulder joint. */
     public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(12.5);
@@ -73,25 +64,16 @@ public static class ElbowMotorConstants {
     /** Elbow motor CAN. */
     public static final CAN CAN = new CAN(32); // TODO
 
-    /** Gearing between the elbow motor and the elbow joint. */
-    private static final double GEARING = 39.29411765;
-
-    /** Moment of inertia of the shoulder, in kilograms meters squared. */
-    private static final double MOI = 0.5713;
-
-    /** Mass of the shoulder joint in kilograms. */
-    private static final double MASS = Units.lbsToKilograms(13.006);
-
-    /** Distance between the shoulder joint and the shoulder joint's center of mass in meters. */
-    private static final double RADIUS = Units.inchesToMeters(12.25179915);
-
-    /** Elbow pivot to wrist pivot distance in meters. */
-    private static final double ELBOW_TO_WRIST_DISTANCE = Units.inchesToMeters(16.825);
-
-    /** Joint constants for the shoulder joint. */
+    /** Joint constants for the elbow joint. */
     public static final JointConstants JOINT_CONSTANTS =
         new JointConstants(
-            MASS, ELBOW_TO_WRIST_DISTANCE, RADIUS, MOI, GEARING, DCMotor.getNEO(1), 1);
+            Units.lbsToKilograms(13.006), // massKg
+            Units.inchesToMeters(16.825), // lengthMeters
+            Units.inchesToMeters(12.251799915), // radiusMeters
+            0.5713,
+            39.2911765,
+            DCMotor.getNEO(1), // motor
+            1);
 
     /** Minimum angle of the elbow joint. */
     public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-90);

src/main/java/frc/robot/arm/ElbowMotorIOSim.java

@@ -24,8 +24,8 @@ public ElbowMotorIOSim() {
         new SingleJointedArmSim(
             ElbowMotorConstants.JOINT_CONSTANTS.motor(),
             ElbowMotorConstants.JOINT_CONSTANTS.gearing(),
-            ElbowMotorConstants.JOINT_CONSTANTS.moi(),
-            ElbowMotorConstants.JOINT_CONSTANTS.length(),
+            ElbowMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(),
+            ElbowMotorConstants.JOINT_CONSTANTS.lengthMeters(),
             ElbowMotorConstants.MINIMUM_ANGLE.getRadians(),
             ElbowMotorConstants.MAXIMUM_ANGLE.getRadians(),
             false,

src/main/java/frc/robot/arm/ShoulderMotorIOSim.java

@@ -24,8 +24,8 @@ public ShoulderMotorIOSim() {
         new SingleJointedArmSim(
             ShoulderMotorConstants.JOINT_CONSTANTS.motor(),
             ShoulderMotorConstants.JOINT_CONSTANTS.gearing(),
-            ShoulderMotorConstants.JOINT_CONSTANTS.moi(),
-            ShoulderMotorConstants.JOINT_CONSTANTS.length(),
+            ShoulderMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(),
+            ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(),
             ShoulderMotorConstants.MINIMUM_ANGLE.getRadians(),
             ShoulderMotorConstants.MAXIMUM_ANGLE.getRadians(),
             false,