feat: create simulated arm pivot component

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/arm/ArmPivotIOSim.java

@@ -0,0 +1,78 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.robot.subsystems.arm;
+
+import static edu.wpi.first.units.Units.Centimeters;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static org.frc6423.robot.subsystems.arm.Arm.*;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import org.frc6423.robot.Constants;
+
+/** Simulated version of the {@link ArmPivotIOReal} class */
+public class ArmPivotIOSim implements ArmPivotIO {
+  private final DCMotor pivotModel = DCMotor.getKrakenX60Foc(1);
+  private final SingleJointedArmSim pivotSim =
+      new SingleJointedArmSim(
+          pivotModel,
+          GEARING,
+          MOI.in(KilogramSquareMeters),
+          LENGTH.in(Centimeters),
+          MIN_ANGLE.in(Radians),
+          MAX_ANGLE.in(Radians),
+          true,
+          0); // TODO measurement std devs arg
+
+  private final ArmFeedforward feedforward = new ArmFeedforward(S, G, V, A);
+  private final ProfiledPIDController feedback =
+      new ProfiledPIDController(
+          P,
+          I,
+          D,
+          new TrapezoidProfile.Constraints(
+              MAX_VELOCITY.in(RadiansPerSecond), MAX_ACCELERATION.in(RadiansPerSecondPerSecond)));
+
+  @Override
+  public double angle() {
+    return pivotSim.getAngleRads();
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    // Battery cannot output more than 12.0v
+    pivotSim.setInputVoltage(MathUtil.clamp(volts, -12.0, 12.0));
+    pivotSim.update(Constants.Flags.LOOPTIME.in(Seconds));
+  }
+
+  @Override
+  public void setAngle(double angleRads) {
+    // Clamp goal angle within range
+    angleRads = MathUtil.clamp(angleRads, MIN_ANGLE.in(Radians), MAX_ANGLE.in(Radians));
+    // Get velocity of previous setpoint
+    double lastVel = feedback.getSetpoint().velocity;
+
+    // Calculate new open-loop and closed-loop output
+    double fbOut = feedback.calculate(angle(), angleRads);
+    double ffOut =
+        feedforward.calculateWithVelocities(angleRads, lastVel, feedback.getSetpoint().velocity);
+
+    // Sum output
+    setVoltage(fbOut + ffOut);
+  }
+
+  @Override
+  public void close() {}
+}