add ratios and MOI for each subsystem

rhr_docs/ARCHITECTURE.md

@@ -2,7 +2,7 @@
 
 Subsystem folders should contain related mechanisms. For example, an `intake` subsystem folder could contain a four-bar mechanism and a roller mechanism.
 > [!NOTE]
-> Note the difference in terminology between WPILib subsystems and as they are documented here and by the CAD team. We group different WPILib "subsystems" for simplicity. All mechanisms should still at some point extend WPILib's `SubsystemBase`. 
+> Note the difference in terminology between WPILib subsystems and as they are documented here and by the CAD team. We group different WPILib "subsystems" for simplicity. All mechanisms should still at some point extend WPILib's `SubsystemBase`.
 
 ## Layout
 ```

src/main/java/frc2713/lib/dynamics/MoiUnits.java

@@ -0,0 +1,39 @@
+package frc2713.lib.dynamics;
+
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.derive;
+
+import edu.wpi.first.units.MomentOfInertiaUnit;
+
+/**
+ * Extended moment of inertia units not included in {@link edu.wpi.first.units.Units}.
+ *
+ * <p>These units integrate with the WPILib units system. Use {@link #PoundSquareInches} with {@code
+ * .of()} and {@code .in(KilogramSquareMeters)} for conversions.
+ *
+ * <p>Conversion: 1 lb·in² = 0.45359237 kg × (0.0254 m)² ≈ 2.926397e-4 kg·m²
+ */
+public final class MoiUnits {
+
+  private MoiUnits() {}
+
+  /**
+   * Moment of inertia in lb²·in (pound squared inches). Common in imperial mechanical specs.
+   *
+   * <p>Example usage:
+   *
+   * <pre>{@code
+   * MomentOfInertia moi = PoundSquareInches.of(100);
+   * double kg2M = moi.in(KilogramSquareMeters);
+   * }</pre>
+   */
+  public static final MomentOfInertiaUnit PoundSquareInches =
+      derive(KilogramSquareMeters)
+          .aggregate(0.45359237 * 0.0254 * 0.0254)
+          .named("PoundSquareInch")
+          .symbol("lb²·in")
+          .make();
+
+  /** Alias for {@link #PoundSquareInches}. */
+  public static final MomentOfInertiaUnit PoundSquareInch = PoundSquareInches;
+}

src/main/java/frc2713/lib/io/SimTalonFXIO.java

@@ -2,6 +2,7 @@
 
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
@@ -44,7 +45,9 @@ public SimTalonFXIO(TalonFXSubsystemConfig config) {
         config,
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(
-                DCMotor.getKrakenX60Foc(1), config.momentOfInertia, config.unitToRotorRatio),
+                DCMotor.getKrakenX60Foc(1),
+                config.momentOfInertia.in(KilogramSquareMeters),
+                config.unitToRotorRatio),
             DCMotor.getKrakenX60Foc(1),
             0.001,
             0.001));

src/main/java/frc2713/lib/subsystem/TalonFXSubsystemConfig.java

@@ -1,9 +1,12 @@
 package frc2713.lib.subsystem;
 
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.measure.MomentOfInertia;
 import frc2713.lib.drivers.CANDeviceId;
 
 public class TalonFXSubsystemConfig {
@@ -26,7 +29,7 @@ public class TalonFXSubsystemConfig {
   public double kMaxPositionUnits = Double.POSITIVE_INFINITY;
 
   // Moment of Inertia (KgMetersSquared) for sim
-  public double momentOfInertia = 0.5;
+  public MomentOfInertia momentOfInertia = KilogramSquareMeters.of(.5);
 
   public Transform3d initialTransform =
       new Transform3d(new Translation3d(0, 0, 0), new Rotation3d());

src/main/java/frc2713/lib/util/Util.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import frc2713.robot.Robot;
 
 public class Util {
   public static final double EPSILON = 1e-12;
@@ -37,4 +38,12 @@ public static Angle fieldToRobotRelative(Angle robotRelative, Pose2d robotPose)
     Rotation2d converted = new Rotation2d(robotRelative.in(Radians)).minus(robotPose.getRotation());
     return converted.getMeasure();
   }
+
+  public static <T> T modeDependentValue(T real, T sim) {
+    return Robot.isReal() ? real : sim;
+  }
+
+  public static <T> T modeDependentValue(T real) {
+    return modeDependentValue(real, real);
+  }
 }

src/main/java/frc2713/robot/subsystems/drive/DriveConstants.java

@@ -2,6 +2,8 @@
 
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
+import edu.wpi.first.units.measure.MomentOfInertia;
+import frc2713.lib.dynamics.MoiUnits;
 import frc2713.lib.util.LoggedTunableGains;
 import frc2713.lib.util.LoggedTunableNumber;
 
@@ -18,4 +20,9 @@ public final class AutoConstants {
 
   public static final LoggedTunableNumber speedScalar =
       new LoggedTunableNumber("Drive/Drive Speed Scalar", 1);
+
+  public static final MomentOfInertia intakeExtendedMoi =
+      MoiUnits.PoundSquareInches.of(29546.954784);
+  public static final MomentOfInertia intakeRetractedMoi =
+      MoiUnits.PoundSquareInches.of(25819.326619);
 }

src/main/java/frc2713/robot/subsystems/intake/IntakeConstants.java

@@ -1,6 +1,8 @@
 package frc2713.robot.subsystems.intake;
 
 import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Pounds;
 import static edu.wpi.first.units.Units.Volts;
 
 import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
@@ -14,10 +16,11 @@
 import com.ctre.phoenix6.mechanisms.DifferentialMotorConstants;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Mass;
 import edu.wpi.first.units.measure.Voltage;
 import frc2713.lib.drivers.CANDeviceId;
+import frc2713.lib.dynamics.MoiUnits;
 import frc2713.lib.subsystem.DifferentialSubsystemConfig;
 import frc2713.lib.subsystem.TalonFXSubsystemConfig;
 import frc2713.lib.util.LoggedTunableMeasure;
@@ -31,8 +34,9 @@ public static final class Roller {
     static {
       config.name = "Intake Roller";
       config.talonCANID = new CANDeviceId(9); // Example CAN ID, replace with actual ID
-      config.unitToRotorRatio = 1.0; // 1:1 ratio
-      config.momentOfInertia = 0.001; // Low MOI for fast-spinning rollers
+      config.unitToRotorRatio = 12.0 / 24.0; // 12 tooth pinion to 24 tooth gear for 0.5 reduction
+      config.momentOfInertia =
+          MoiUnits.PoundSquareInches.of(0.295439).times(2); // Low MOI for fast-spinning rollers
     }
 
     public static Voltage intakeVoltageDesired = Volts.of(5.0);
@@ -45,6 +49,11 @@ public static final class Extension {
     public static DifferentialSubsystemConfig differentialConfig =
         new DifferentialSubsystemConfig();
     public static final double averageGearRatio = 60.0 / 9.0;
+    public static final Mass movingMass = Pounds.of(11.75);
+    public static final Distance sprocketPitchDiameter =
+        Inches.of(
+            1.273); // Diameter of the circle formed by the center of the sprocket teeth, used for
+    // calculating distance per rotation
 
     static {
       var avgGains =
@@ -64,11 +73,13 @@ public static final class Extension {
       config.unitToRotorRatio = 1.0; // assumes 1:1 gearbox
       config.unitRotationsPerMeter =
           averageGearRatio
-              * Units.inchesToMeters(1.273)
+              * sprocketPitchDiameter.in(Meters)
               * Math.PI; // gearRatio * sprocketPitchDiameter * pi
 
-      // Moment of inertia for sim - reasonable for light linear mechanism
-      config.momentOfInertia = 0.01;
+      // MOI = m*r^2, where r is the radius to the center of mass (half the pitch diameter)
+      config.momentOfInertia =
+          MoiUnits.PoundSquareInches.of(
+              movingMass.in(Pounds) * Math.pow(sprocketPitchDiameter.in(Inches) / 2.0, 2));
       config.tunable = true;
 
       // Differential configuration

src/main/java/frc2713/robot/subsystems/launcher/Flywheels.java

@@ -207,7 +207,7 @@ public void launchFuel(LaunchSolution solution) {
     // Enforce max fire rate by checking time since last launch. If we haven't waited long enough,
     // skip this launch.
     if (now.minus(lastLaunchTime).in(Seconds)
-        >= (1.0 / LauncherConstants.Flywheels.LaunchRateFuelPerSecond)) {
+        >= (1.0 / LauncherConstants.Flywheels.launchRateFuelPerSecond)) {
       lastLaunchTime = now;
 
       this.fuelTrajectories.launch(

src/main/java/frc2713/robot/subsystems/launcher/LauncherConstants.java

@@ -17,11 +17,14 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.units.measure.Time;
 import frc2713.lib.drivers.CANDeviceId;
+import frc2713.lib.dynamics.MoiUnits;
 import frc2713.lib.subsystem.TalonFXSubsystemConfig;
 import frc2713.lib.util.LoggedTunableBoolean;
 import frc2713.lib.util.LoggedTunableMeasure;
+import frc2713.lib.util.Util;
 import frc2713.robot.subsystems.launcher.turretIO.TurretSubsystemConfig;
 
 public final class LauncherConstants {
@@ -53,7 +56,7 @@ public static final class Turret {
 
       // Gear ratio: motor rotations per turret rotation = GEAR_1/GEAR_0 = 120/60 = 2.0
       config.unitToRotorRatio = 120.0 / 60.0;
-      config.momentOfInertia = 0.02; // kg*m^2 for simulation
+      config.momentOfInertia = MoiUnits.PoundSquareInches.of(522.908341);
 
       config.initialTransform =
           new Transform3d(
@@ -67,9 +70,24 @@ public static final class Turret {
     public static int MODEL_INDEX = 3;
     public static int PARENT_INDEX = 0; // drivetrain
 
+    // Gear tooth counts for calculating overall gear ratio
+    public static final double pinionGearTeeth = 15.0;
+    public static final double spurGear1Teeth = 20.0;
+    public static final double sprocketPinionTeeth = 16.0;
+    public static final double sprocketGearTeeth = 224.0;
+
+    // Overall gear ratio from motor rotations to turret rotations
+    // motor has an absolute encoder, so this can be encoder 1
+    public static final double motorToTurretGearRatio =
+        (spurGear1Teeth / pinionGearTeeth) * (sprocketGearTeeth / sprocketPinionTeeth);
+
+    // Gear ratio from encoder rotations to turret rotations (encoder is after the first stage
+    // reduction)
+    public static final double encoderToTurretGearRatio = sprocketGearTeeth / sprocketPinionTeeth;
+
     // Gear tooth counts for turret angle calculation
     // Pinion on motor
-    public static final double GEAR_0_TOOTH_COUNT = 60.0; // TODO: Replace with actual value
+    public static final double GEAR_0_TOOTH_COUNT = 15.0; // TODO: Replace with actual value
     // attached to e1
     public static final double GEAR_1_TOOTH_COUNT = 120.0; // TODO: Replace with actual value
     // attached to e2
@@ -79,6 +97,7 @@ public static final class Turret {
             / ((GEAR_1_TOOTH_COUNT - GEAR_2_TOOTH_COUNT) * GEAR_0_TOOTH_COUNT);
 
     // How many times encoder 1 spins per 1 degree of turret rotation
+    // TODO: update this with the actual ratios from above.
     public static final double ENCODER_1_TO_TURRET_RATIO = GEAR_1_TOOTH_COUNT / GEAR_0_TOOTH_COUNT;
 
     // Turret rotation limits
@@ -94,33 +113,35 @@ public final class Flywheels {
 
     public static TalonFXSubsystemConfig leaderConfig = new TalonFXSubsystemConfig();
     public static TalonFXSubsystemConfig followerConfig = new TalonFXSubsystemConfig();
+    public static MomentOfInertia flywhMomentOfInertia = MoiUnits.PoundSquareInches.of(10.410164);
+    public static double gearRatio = 24.0 / 18.0; // 1.33:1 reduction from motor to flywheel
 
     static {
       leaderConfig.name = "Flywheel Leader";
       leaderConfig.talonCANID = new CANDeviceId(2); // Example CAN ID, replace with actual ID
-      leaderConfig.fxConfig.Slot0.kP = 0.3;
+      leaderConfig.fxConfig.Slot0.kP = Util.modeDependentValue(0.3, 3.5);
       leaderConfig.fxConfig.Slot0.kI = 0.0;
       leaderConfig.fxConfig.Slot0.kD = 0.0;
       leaderConfig.fxConfig.Slot0.kS = 0.15;
       leaderConfig.fxConfig.Slot0.kV = 0.114;
-      leaderConfig.unitToRotorRatio = 1.0; // 1:1 ratio
+      leaderConfig.unitToRotorRatio = gearRatio; // 1.33:1 reduction from motor to flywheel
       leaderConfig.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
       leaderConfig.fxConfig.MotorOutput.PeakReverseDutyCycle = 0;
-      leaderConfig.momentOfInertia = 0.01;
+      leaderConfig.momentOfInertia = flywhMomentOfInertia.times(0.5);
       leaderConfig.useFOC = false; // Use VelocityVoltage for sim compatibility
       leaderConfig.tunable = true;
 
       followerConfig.name = "Flywheel Follower";
       followerConfig.talonCANID = new CANDeviceId(1); // Example CAN ID, replace with actual ID
-      followerConfig.unitToRotorRatio = 1.0; // 1:1 ratio
-      followerConfig.momentOfInertia = 0.01;
+      followerConfig.unitToRotorRatio = gearRatio; // 1.33:1 reduction from motor to flywheel
+      followerConfig.momentOfInertia = flywhMomentOfInertia.times(0.5);
       followerConfig.useFOC = false;
     }
 
     public static int MODEL_INDEX = 5;
     public static int PARENT_INDEX = 4;
 
-    public static AngularVelocity acceptableError = RPM.of(20);
+    public static AngularVelocity acceptableError = RPM.of(35);
     public static AngularVelocity idleVelocity = RotationsPerSecond.of(20);
 
     public static Transform3d localTransform =
@@ -133,7 +154,7 @@ public final class Flywheels {
 
     public static Distance WHEEL_DIAMETER = Inches.of(4);
     // How many fuel we can launch per second at max firing rate
-    public static double LaunchRateFuelPerSecond = 9.0;
+    public static double launchRateFuelPerSecond = 9.0;
 
     static {
       // Distance (m) -> Ball Velocity (ft/s)
@@ -152,6 +173,10 @@ public final class Hood {
     public static TalonFXSubsystemConfig config = new TalonFXSubsystemConfig();
     public static Angle acceptableError = Degrees.of(5);
 
+    // 9 tooth pinion to 20 tooth gear, 16 tooth gear to 38 tooth gear, 10 tooth gear to 124 tooth
+    // gear for total reduction of 0.0306
+    public static double gearRatio = (9.0 / 20.0) * (16.0 / 38.0) * (10.0 / 124.0);
+
     static {
       config.name = "Hood";
       config.talonCANID = new CANDeviceId(14); // Example CAN ID, replace with actual ID
@@ -169,8 +194,8 @@ public final class Hood {
       config.fxConfig.MotionMagic.MotionMagicAcceleration = 4.0; // rotations per second^2
       config.fxConfig.MotionMagic.MotionMagicJerk = 0; // no jerk limit
 
-      config.unitToRotorRatio = 1.0; // 1:1 ratio
-      config.momentOfInertia = 0.005; // kg*m^2 for simulation
+      config.unitToRotorRatio = gearRatio;
+      config.momentOfInertia = MoiUnits.PoundSquareInches.of(38.979757);
 
       config.initialTransform =
           new Transform3d(
@@ -179,12 +204,12 @@ public final class Hood {
     }
 
     public static Angle retractedPosition = Degrees.of(0);
-    public static Angle extendedPosition = Degrees.of(90);
+    public static Angle extendedPosition = Degrees.of(30);
     public static int MODEL_INDEX = 4;
     public static int PARENT_INDEX = 3; // turret
 
     // Hood rotation limits
-    public static final double FORWARD_LIMIT_DEGREES = 60;
+    public static final double FORWARD_LIMIT_DEGREES = 30;
     public static final double REVERSE_LIMIT_DEGREES = 0;
 
     public static Angle staticHubAngle = Degree.of(10);

src/main/java/frc2713/robot/subsystems/serializer/SerializerConstants.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.measure.AngularVelocity;
 import frc2713.lib.drivers.CANDeviceId;
+import frc2713.lib.dynamics.MoiUnits;
 import frc2713.lib.subsystem.TalonFXSubsystemConfig;
 import frc2713.lib.util.LoggedTunableMeasure;
 
@@ -34,8 +35,12 @@ public static final class DyeRotor {
       config.fxConfig.Slot0.kV = 0.12; // Volts per rps (feedforward)
 
       config.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-      config.unitToRotorRatio = 46.0 / 14.0;
-      config.momentOfInertia = 0.001; // kg*m^2 for simulation (small roller)
+      config.unitToRotorRatio =
+          (8.0 / 44.0)
+              * (12.0
+                  / 114.0); // 8 tooth pinion to 44 tooth gear, 12 tooth gear to 114 tooth gear for
+      // total reduction of 0.0195
+      config.momentOfInertia = MoiUnits.PoundSquareInches.of(89.2780792);
 
       config.initialTransform =
           new Transform3d(
@@ -58,7 +63,6 @@ public static final class Feeder {
     static {
       config.name = "Feeder";
       config.talonCANID = new CANDeviceId(44); // Example CAN ID, replace with actual ID
-
       config.useFOC = false;
 
       // Velocity PID gains for VelocityVoltage control
@@ -70,8 +74,11 @@ public static final class Feeder {
       config.fxConfig.Slot0.kV = 0.12; // Volts per rps (feedforward)
 
       config.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-      config.unitToRotorRatio = 1.0;
-      config.momentOfInertia = 0.001; // kg*m^2 for simulation
+      config.unitToRotorRatio =
+          14.0 / 37.0; // 14 tooth gear on motor to 37 tooth gear on feeder roller
+      // tough to estimate this, but I just determined the MOI of the wheel assembly by the hook and
+      // tripled it
+      config.momentOfInertia = MoiUnits.PoundSquareInches.of(0.075 * 3);
     }
 
     public static LoggedTunableMeasure<AngularVelocity> shootingSpeed =