From 7ed98e2362f97a7740cffb451f10826979116e54 Mon Sep 17 00:00:00 2001
From: Hayden Heroux <haydenheroux@gmail.com>
Date: Fri, 19 Jan 2024 11:30:26 -0500
Subject: [PATCH] fix(swerve): Use steer PIDF for simulated steer motor.

---
 simgui.json                                   | 10 ++++----
 .../frc/robot/swerve/SteerMotorIOSim.java     | 24 ++++++++++++++-----
 2 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/simgui.json b/simgui.json
index f456d10..0e20516 100644
--- a/simgui.json
+++ b/simgui.json
@@ -15,10 +15,13 @@
   "NetworkTables": {
     "transitory": {
       "Shuffleboard": {
-        "Swerve": {
-          "Module 0": {
+        "Odometry": {
+          "Velocity": {
             "open": true
           },
+          "open": true
+        },
+        "Swerve": {
           "Module 1": {
             "open": true
           },
@@ -30,8 +33,7 @@
           },
           "North East Module": {
             "open": true
-          },
-          "open": true
+          }
         },
         "open": true
       },
diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOSim.java b/src/main/java/frc/robot/swerve/SteerMotorIOSim.java
index d2d14d2..4fef638 100644
--- a/src/main/java/frc/robot/swerve/SteerMotorIOSim.java
+++ b/src/main/java/frc/robot/swerve/SteerMotorIOSim.java
@@ -1,9 +1,10 @@
 package frc.robot.swerve;
 
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import frc.lib.PIDFConstants;
 import frc.robot.RobotConstants;
 import frc.robot.swerve.SwerveConstants.MK4iConstants;
 
@@ -23,15 +24,23 @@ public class SteerMotorIOSim implements SteerMotorIO {
   /** Represents the velocity of the steer motor. */
   private double velocityRotationsPerSecond;
 
-  /** Feedback controller for the position. */
-  // TODO
-  private final PIDController positionFeedback = new PIDController(1.0, 0, 0.25);
+  /** Constants for PIDF position controller. */
+  private static PIDFConstants pidfConstants = new PIDFConstants();
+
+  static {
+    pidfConstants.kP = 1.0; // volts per rotation
+    pidfConstants.kVelocityConstraint = 10.0; // volts per rotation per second
+    pidfConstants.kAccelerationConstraint = 64.0; // volts per rotation per second per second
+  }
+
+  /** PIDF position controller. */
+  private final SteerMotorPIDF pidf;
 
   public SteerMotorIOSim() {
     positionRotations = 0.0;
     velocityRotationsPerSecond = 0.0;
 
-    positionFeedback.enableContinuousInput(0, 1);
+    pidf = new SteerMotorPIDF(pidfConstants);
   }
 
   @Override
@@ -50,7 +59,10 @@ public void setPosition(double positionRotations) {
 
   @Override
   public void setSetpoint(double positionRotations) {
-    double voltage = positionFeedback.calculate(this.positionRotations, positionRotations);
+    double voltage =
+        pidf.calculate(
+            Rotation2d.fromRotations(this.positionRotations),
+            Rotation2d.fromRotations(positionRotations));
 
     wheelSim.setInputVoltage(voltage);
     wheelSim.update(RobotConstants.PERIODIC_DURATION);