high hz simulation, gains should be good

Signed-off-by: Jade Turner <spacey-sooty@proton.me>

Author: spacey-sooty

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOComp.java

@@ -50,8 +50,8 @@ public ElevatorIOComp() {
     slot0.GravityType = GravityTypeValue.Elevator_Static;
     // we lie about units cause of how the dimensional analysis works out
     slot0.kG = 0.09;
-    slot0.kP = rotationsToMetres(19.11);
-    slot0.kD = rotationsToMetres(0.05);
+    slot0.kP = rotationsToMetres(318.70);
+    slot0.kD = rotationsToMetres(1.33);
     slot0.kV = rotationsToMetres(5.29);
     slot0.kV = rotationsToMetres(0.01);
 

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java

@@ -1,16 +1,34 @@
 package org.curtinfrc.frc2025.subsystems.elevator;
 
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 
 public class ElevatorIOSim extends ElevatorIOComp {
   private final ElevatorSim elevatorSim =
-      // new ElevatorSim(3.35, 0.02, DCMotor.getKrakenX60Foc(2), 0, 0.6, true, 0.01);
       new ElevatorSim(5.29, 0.01, DCMotor.getKrakenX60Foc(2), -1, 1, true, 0, 0.001, 0.01);
 
-  @Override
-  public void updateInputs(ElevatorIOInputs inputs) {
-    super.updateInputs(inputs);
+  private double lastSimTime;
+  private final Notifier notifier;
+
+  public ElevatorIOSim() {
+    super();
+    lastSimTime = RobotController.getFPGATime();
+
+    notifier =
+        new Notifier(
+            () -> {
+              final double currentTime = RobotController.getFPGATime();
+              double deltaTime = currentTime - lastSimTime;
+              lastSimTime = currentTime;
+
+              updateSimState(deltaTime);
+            });
+    notifier.startPeriodic(0.001);
+  }
+
+  private void updateSimState(double deltaTime) {
     var simState = motor.getSimState();
     elevatorSim.setInput(simState.getMotorVoltage());
     elevatorSim.update(0.02);
@@ -19,4 +37,13 @@ public void updateInputs(ElevatorIOInputs inputs) {
     var rotorVel = metresToRotations(elevatorSim.getVelocityMetersPerSecond());
     simState.setRotorVelocity(rotorVel);
   }
+
+  @Override
+  public void updateInputs(ElevatorIOInputs inputs) {
+    super.updateInputs(inputs);
+    final double currentTime = RobotController.getFPGATime();
+    double deltaTime = currentTime - lastSimTime;
+    lastSimTime = currentTime;
+    updateSimState(deltaTime);
+  }
 }