Repository: FRC2713/Robot2025
Pull request #10: Update gains to include motion magic, disable continuous wrapping.
Author: login: tytremblay
Created: 2025-03-02T22:19:18Z | Merged: 2025-03-03T17:29:46Z

- Move to FusedCANCoder for position sensors.
- Moves all kraken configs into out of constants and into their appropriate IO class.
- Disables continuous wrap on all mechanisms (setpoints will need to change because direction matters now)
- Update gains to handle motion magic features.


Diff:
diff --git a/src/main/java/frc/robot/commands/ShoulderCmds.java b/src/main/java/frc/robot/commands/ShoulderCmds.java
index e9318095..cec21623 100644
--- a/src/main/java/frc/robot/commands/ShoulderCmds.java
+++ b/src/main/java/frc/robot/commands/ShoulderCmds.java
@@ -29,4 +29,5 @@ public static Command setAngleAndWait(double targetAngle) {
   public static Command setAngleAndWait(DoubleSupplier targetAngle) {
     return Commands.sequence(setAngle(targetAngle), waitUntilAtTarget());
   }
+
 }
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java
index ff3738ae..c43e0438 100644
--- a/src/main/java/frc/robot/generated/TunerConstants.java
+++ b/src/main/java/frc/robot/generated/TunerConstants.java
@@ -13,15 +13,15 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.*;
 import frc.robot.util.ControlGains;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 
 // Generated by the Tuner X Swerve Project Generator
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
 public class TunerConstants {
-  public static final LoggedTunablePID steerGainsPID =
-      new LoggedTunablePID("Module/Steer", new ControlGains().p(100.).d(0.5).s(0.1).v(2.66));
-  public static final LoggedTunablePID driveGainsPID =
-      new LoggedTunablePID("Module/Drive", new ControlGains().p(2.5).d(0.).s(0.21478).v(0.82837));
+  public static final LoggedTunableGains steerGainsPID =
+      new LoggedTunableGains("Module/Steer", new ControlGains().p(100.).d(0.5).s(0.1).v(2.66));
+  public static final LoggedTunableGains driveGainsPID =
+      new LoggedTunableGains("Module/Drive", new ControlGains().p(2.5).d(0.).s(0.21478).v(0.82837));
 
   // Both sets of gains need to be tuned to your individual robot.
 
diff --git a/src/main/java/frc/robot/subsystems/constants/DriveConstants.java b/src/main/java/frc/robot/subsystems/constants/DriveConstants.java
index 27be3e45..fc58e87c 100644
--- a/src/main/java/frc/robot/subsystems/constants/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/constants/DriveConstants.java
@@ -41,14 +41,14 @@ public final class HeadingControllerConstants {
         new ControlGains()
             .p(5.0)
             .d(0.4)
-            .trapezoidal(kMaxAngularVelocity, kMaxAngularAcceleration)
+            .trapezoidal(kMaxAngularVelocity, kMaxAngularAcceleration, 0)
             .createTrapezoidalPIDController();
 
     public static final ProfiledPIDController angleController =
         new ControlGains()
             .p(5.0)
             .d(0.4)
-            .trapezoidal(kMaxAngularVelocity, kMaxAngularAcceleration)
+            .trapezoidal(kMaxAngularVelocity, kMaxAngularAcceleration, 0)
             .createTrapezoidalPIDController();
   }
 
diff --git a/src/main/java/frc/robot/subsystems/constants/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/constants/ElevatorConstants.java
index 58337c2e..9b2c369b 100644
--- a/src/main/java/frc/robot/subsystems/constants/ElevatorConstants.java
+++ b/src/main/java/frc/robot/subsystems/constants/ElevatorConstants.java
@@ -1,15 +1,12 @@
 package frc.robot.subsystems.constants;
 
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.util.ControlGains;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import frc.robot.util.RHRUtil;
 
 public class ElevatorConstants {
@@ -32,9 +29,17 @@ public class ElevatorConstants {
   public static final double kRotationsToHeightConversion = (Math.PI * kDrumRadius * 2);
 
   public static final double kMinHeight = 0.0; // inches
-  public static final double kMaxHeight = 44.7; // 52.5;
+  public static final double kMaxHeight = 26.0; // inches
   public static final double kInitialHeight = 0;
 
+  public static double toInches(double rotations) {
+    return rotations * kRotationsToHeightConversion;
+  }
+
+  public static double toRotations(double inches) {
+    return inches / kRotationsToHeightConversion;
+  }
+
   // robot frame to elevator frame
   public static final Transform3d kInitialTransform =
       new Transform3d(0.0, 0.0, Units.inchesToMeters(kInitialHeight), new Rotation3d(0, 0, 0));
@@ -42,8 +47,8 @@ public class ElevatorConstants {
   // pose in robot frame
   public static final Pose3d kInitialPose = new Pose3d().transformBy(kInitialTransform);
 
-  public static final LoggedTunablePID PID_LEVEL_ONE =
-      new LoggedTunablePID(
+  public static final LoggedTunableGains Gains =
+      new LoggedTunableGains(
           "Elevator_L_ONE",
           new ControlGains()
               // PID
@@ -55,49 +60,14 @@ public class ElevatorConstants {
               .v(0.0)
               .a(0.0)
               // Motion Magic
-              .mmCruiseVelo(7),
-          160,
-          1600);
-
-  public static final double LEVEL_TWO_HEIGHT_IN = 25.13427520950623;
-
-  public static final LoggedTunablePID PID_LEVEL_TWO =
-      new LoggedTunablePID(
-          "Elevator_L_TWO",
-          new ControlGains()
-              // PID
-              .p(RHRUtil.modeDependentDouble(25., 0.1))
-              .i(0.0)
-              .d(RHRUtil.modeDependentDouble(1, 0.01))
-              // FF
-              .g(RHRUtil.modeDependentDouble(15, 0.0785))
-              .v(0.0)
-              .a(0.0));
+              .trapezoidal(
+                  RHRUtil.modeDependentDouble(0.5, 0.1),
+                  RHRUtil.modeDependentDouble(0.5, 0.1),
+                  RHRUtil.modeDependentDouble(0.5, 0.1))
+              .exponential(6.4, 0.1));
 
   public static final double mech2dWidth = Units.inchesToMeters(1);
   public static final Color8Bit mech2dColor = new Color8Bit(255, 0, 0);
 
-  public static TalonFXConfiguration createKrakenConfig(boolean inverted) {
-    var config = new TalonFXConfiguration();
-
-    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    config.Feedback.SensorToMechanismRatio = ElevatorConstants.kGearReduction;
-    config.TorqueCurrent.PeakForwardTorqueCurrent = ElevatorConstants.kMaxCurrentLimit;
-    config.TorqueCurrent.PeakReverseTorqueCurrent = -ElevatorConstants.kMaxCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimit = ElevatorConstants.kMaxCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimitEnable = true;
-    config.MotorOutput.Inverted =
-        (inverted) ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive;
-
-    // set slot 0 gains
-    config.Slot0 = PID_LEVEL_ONE.toTalonFX();
-    config.Slot1 = PID_LEVEL_TWO.toTalonFXS1();
-
-    // set Motion Magic settings
-    config.MotionMagic = PID_LEVEL_ONE.toMotionMagic();
-
-    return config;
-  }
-
   public static final double AT_TARGET_GIVE_INCHES = 1.0;
 }
diff --git a/src/main/java/frc/robot/subsystems/constants/PivotConstants.java b/src/main/java/frc/robot/subsystems/constants/PivotConstants.java
index 6e11a1d5..f0f465c4 100644
--- a/src/main/java/frc/robot/subsystems/constants/PivotConstants.java
+++ b/src/main/java/frc/robot/subsystems/constants/PivotConstants.java
@@ -1,18 +1,12 @@
 package frc.robot.subsystems.constants;
 
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.signals.GravityTypeValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.util.ControlGains;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import frc.robot.util.RHRUtil;
 
 public class PivotConstants {
@@ -24,7 +18,7 @@ public class PivotConstants {
   public static final double kLength = Units.inchesToMeters(18);
   public static final double kMass = 7.094328; // kg
 
-  public static final double kMinAngleRad = Units.degreesToRadians(-180);
+  public static final double kMinAngleRad = Units.degreesToRadians(0);
   public static final double kMaxAngleRad = Units.degreesToRadians(100);
 
   public static final double kInitialAngleRad = Units.degreesToRadians(35);
@@ -54,12 +48,14 @@ public class PivotConstants {
   public static final double kA = 0.0;
   public static final double kS = RHRUtil.modeDependentDouble(10., 0.); // Volts
 
-  public static final double kTrapezoidalMaxVelocity = .1;
-  public static final double kTrapezoidalMaxAcceleration = 10;
-  public static final double kTrapezoidalMaxJerk = 100;
+  public static final double kTrapezoidalMaxVelocity = .05;
+  public static final double kTrapezoidalMaxAcceleration = 60;
+  public static final double kTrapezoidalMaxJerk = 300;
+  public static final double kExponential_kV = 6.4;
+  public static final double kExponential_kA = 0.1;
 
-  public static final LoggedTunablePID PID =
-      new LoggedTunablePID(
+  public static final LoggedTunableGains Gains =
+      new LoggedTunableGains(
           "Pivot",
           new ControlGains()
               // PID
@@ -71,10 +67,11 @@ public class PivotConstants {
               .s(kS)
               .v(kV)
               .a(kA)
-              // Motion Magic
-              .mmCruiseVelo(1),
-          80,
-          1600);
+              .maxTrapezoidalVelocity(kTrapezoidalMaxVelocity)
+              .maxTrapezoidalAcceleration(kTrapezoidalMaxAcceleration)
+              .maxTrapezoidalJerk(kTrapezoidalMaxJerk)
+              .expo_kV(kExponential_kV)
+              .expo_kA(kExponential_kA));
 
   public static final double kAbsoluteEncoderOffset = Units.degreesToRotations(175.166);
   public static final double humanOffsetDegs = -55;
@@ -82,37 +79,5 @@ public class PivotConstants {
   public static final int mech2dWidth = 10;
   public static final Color8Bit mech2dColor = new Color8Bit(0, 0, 255);
 
-  public static TalonFXConfiguration createKrakenConfig() {
-    var config = new TalonFXConfiguration();
-    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    config.Feedback.SensorToMechanismRatio = PivotConstants.kGearing;
-    config.Feedback.RotorToSensorRatio = 1;
-    config.TorqueCurrent.PeakForwardTorqueCurrent = PivotConstants.kStallCurrentLimit;
-    config.TorqueCurrent.PeakReverseTorqueCurrent = -PivotConstants.kStallCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimit = PivotConstants.kStatorCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimitEnable = true;
-    config.MotorOutput.Inverted =
-        (PivotConstants.kInverted)
-            ? InvertedValue.CounterClockwise_Positive
-            : InvertedValue.Clockwise_Positive;
-
-    config.Slot0 = PID.toTalonFX(GravityTypeValue.Arm_Cosine);
-    // config.MotionMagic = PID.toMotionMagic();
-    config.MotionMagic.MotionMagicCruiseVelocity = kTrapezoidalMaxVelocity;
-    config.MotionMagic.MotionMagicAcceleration = kTrapezoidalMaxAcceleration;
-    config.MotionMagic.MotionMagicJerk = kTrapezoidalMaxJerk;
-    config.MotionMagic.MotionMagicExpo_kV = 6.4;
-    config.MotionMagic.MotionMagicExpo_kA = 0.1;
-    return config;
-  }
-
-  public static CANcoderConfiguration createCaNcoderConfiguration() {
-    var config = new CANcoderConfiguration();
-    config.MagnetSensor.MagnetOffset = kAbsoluteEncoderOffset;
-    config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
-
-    return config;
-  }
-
-  public static final double AT_TARGET_GIVE_DEGS = 7;
+  public static final double AT_TARGET_GIVE_DEGS = 1;
 }
diff --git a/src/main/java/frc/robot/subsystems/constants/ShoulderConstants.java b/src/main/java/frc/robot/subsystems/constants/ShoulderConstants.java
index fa128622..650d01d4 100644
--- a/src/main/java/frc/robot/subsystems/constants/ShoulderConstants.java
+++ b/src/main/java/frc/robot/subsystems/constants/ShoulderConstants.java
@@ -1,17 +1,12 @@
 package frc.robot.subsystems.constants;
 
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.signals.GravityTypeValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.util.ControlGains;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import frc.robot.util.RHRUtil;
 
 public class ShoulderConstants {
@@ -57,9 +52,11 @@ public class ShoulderConstants {
   public static final double kTrapezoidalMaxVelocity = .1;
   public static final double kTrapezoidalMaxAcceleration = 1;
   public static final double kTrapezoidalMaxJerk = 10;
+  public static final double kExponential_kV = 6.4;
+  public static final double kExponential_kA = 0.1;
 
-  public static final LoggedTunablePID PID =
-      new LoggedTunablePID(
+  public static final LoggedTunableGains Gains =
+      new LoggedTunableGains(
           "Shoulder",
           new ControlGains()
               // PID
@@ -72,46 +69,15 @@ public class ShoulderConstants {
               .v(kV)
               .a(kA)
               // Motion Magic
-              .mmCruiseVelo(1),
-          80,
-          1600);
+              .trapezoidal(
+                  kTrapezoidalMaxVelocity, kTrapezoidalMaxAcceleration, kTrapezoidalMaxJerk)
+              .expo_kV(kExponential_kV)
+              .expo_kA(kExponential_kA));
 
   public static final double kAbsoluteEncoderOffset = Units.degreesToRotations(-208.388);
 
   public static final int mech2dWidth = 10;
   public static final Color8Bit mech2dColor = new Color8Bit(0, 255, 0);
 
-  public static TalonFXConfiguration createKrakenConfig() {
-    var config = new TalonFXConfiguration();
-    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    config.Feedback.SensorToMechanismRatio = ShoulderConstants.kGearing;
-    config.Feedback.RotorToSensorRatio = 1;
-    config.TorqueCurrent.PeakForwardTorqueCurrent = ShoulderConstants.kStallCurrentLimit;
-    config.TorqueCurrent.PeakReverseTorqueCurrent = -ShoulderConstants.kStallCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimit = ShoulderConstants.kStatorCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimitEnable = true;
-    config.MotorOutput.Inverted =
-        (ShoulderConstants.kInverted)
-            ? InvertedValue.CounterClockwise_Positive
-            : InvertedValue.Clockwise_Positive;
-
-    config.Slot0 = PID.toTalonFX(GravityTypeValue.Arm_Cosine);
-    // config.MotionMagic = PID.toMotionMagic();
-
-    config.MotionMagic.MotionMagicCruiseVelocity = kTrapezoidalMaxVelocity;
-    config.MotionMagic.MotionMagicAcceleration = kTrapezoidalMaxAcceleration;
-    config.MotionMagic.MotionMagicJerk = kTrapezoidalMaxJerk;
-    config.MotionMagic.MotionMagicExpo_kV = 6.4;
-    config.MotionMagic.MotionMagicExpo_kA = 0.1;
-    return config;
-  }
-
-  public static CANcoderConfiguration createCANcoderConfiguration() {
-    var config = new CANcoderConfiguration();
-    config.MagnetSensor.MagnetOffset = kAbsoluteEncoderOffset;
-
-    return config;
-  }
-
-  public static final double AT_TARGET_GIVE_DEGS = 4;
+  public static final double AT_TARGET_GIVE_DEGS = 2;
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
index e88e963f..c59402dc 100644
--- a/src/main/java/frc/robot/subsystems/drive/Module.java
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -22,7 +22,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import org.littletonrobotics.junction.Logger;
 
 public class Module {
@@ -146,11 +146,11 @@ public double getFFCharacterizationVelocity() {
     return Units.radiansToRotations(inputs.driveVelocityRadPerSec);
   }
 
-  public void setDrivePID(LoggedTunablePID drivePID) {
+  public void setDrivePID(LoggedTunableGains drivePID) {
     this.io.setDrivePID(drivePID);
   }
 
-  public void setTurnPID(LoggedTunablePID turnPID) {
+  public void setTurnPID(LoggedTunableGains turnPID) {
     this.io.setTurnPID(turnPID);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
index 3ee68997..fbed8463 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
@@ -14,7 +14,7 @@
 package frc.robot.subsystems.drive;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ModuleIO {
@@ -54,7 +54,7 @@ public default void setDriveVelocity(double velocityRadPerSec) {}
   /** Run the turn motor to the specified rotation. */
   public default void setTurnPosition(Rotation2d rotation) {}
 
-  public default void setDrivePID(LoggedTunablePID drivePID) {}
+  public default void setDrivePID(LoggedTunableGains drivePID) {}
 
-  public default void setTurnPID(LoggedTunablePID turnPID) {}
+  public default void setTurnPID(LoggedTunableGains turnPID) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
index 79e1ed64..27effda6 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -43,7 +43,7 @@
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
 import frc.robot.generated.TunerConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import java.util.Queue;
 
 /**
@@ -277,7 +277,7 @@ public void setTurnPosition(Rotation2d rotation) {
   }
 
   @Override
-  public void setDrivePID(LoggedTunablePID pid) {
+  public void setDrivePID(LoggedTunableGains pid) {
     System.out.println("Changing drive PID");
     setDriveConfig(
         new Slot0Configs()
@@ -289,7 +289,7 @@ public void setDrivePID(LoggedTunablePID pid) {
   }
 
   @Override
-  public void setTurnPID(LoggedTunablePID pid) {
+  public void setTurnPID(LoggedTunableGains pid) {
     System.out.println("Changing turn PID");
     setTurnConfig(
         new Slot0Configs()
diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
index 9faa11cf..b1116687 100644
--- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -30,27 +30,11 @@ public Elevator(ElevatorIO IO) {
     this.IO.updateInputs(this.inputs);
   }
 
-  private boolean crossedLevelTwo = false;
-  private boolean crossedLevelOne = true;
-
   @Override
   public void periodic() {
-    if (ElevatorConstants.PID_LEVEL_ONE.hasChanged(hashCode())
-        || ElevatorConstants.PID_LEVEL_TWO.hasChanged(hashCode())) {
-      this.IO.setPID(ElevatorConstants.PID_LEVEL_ONE, ElevatorConstants.PID_LEVEL_TWO);
-    }
-
-    if (getCurrentHeight() > ElevatorConstants.LEVEL_TWO_HEIGHT_IN && !crossedLevelTwo) {
-      this.IO.changeSlot(1);
-      crossedLevelTwo = true;
-      crossedLevelOne = false;
-    } else if (getCurrentHeight() <= ElevatorConstants.LEVEL_TWO_HEIGHT_IN && !crossedLevelOne) {
-      this.IO.changeSlot(0);
-      crossedLevelOne = true;
-      crossedLevelTwo = false;
+    if (ElevatorConstants.Gains.hasChanged(hashCode())) {
+      this.IO.setPID(ElevatorConstants.Gains);
     }
-    Logger.recordOutput(
-        "Elevator/Stage", getCurrentHeight() > ElevatorConstants.LEVEL_TWO_HEIGHT_IN ? 2 : 1);
 
     this.IO.updateInputs(this.inputs);
     Logger.processInputs("Elevator", this.inputs);
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
index 5318d7e2..675190ff 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.elevator;
 
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ElevatorIO {
@@ -35,7 +35,7 @@ public default void setVoltage(double volts1, double volts2) {}
   public default void setTargetHeight(double heightInches) {}
   ;
 
-  public default void setPID(LoggedTunablePID slot0pid, LoggedTunablePID slot1pid) {}
+  public default void setPID(LoggedTunableGains slot0pid) {}
 
   public default void changeSlot(int slot) {}
   ;
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOKrakens.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOKrakens.java
index 4b7bdea3..fd66a883 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOKrakens.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOKrakens.java
@@ -3,8 +3,10 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import frc.robot.subsystems.constants.ElevatorConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import frc.robot.util.PhoenixUtil;
 
 public class ElevatorIOKrakens implements ElevatorIO {
@@ -22,7 +24,7 @@ public ElevatorIOKrakens() {
     left = new TalonFX(ElevatorConstants.kLeftCANId);
     right = new TalonFX(ElevatorConstants.kRightCANId);
 
-    leftConfig = ElevatorConstants.createKrakenConfig(false);
+    leftConfig = createKrakenConfig(false);
     PhoenixUtil.tryUntilOk(5, () -> left.getConfigurator().apply(leftConfig, 0.25));
     PhoenixUtil.tryUntilOk(
         5,
@@ -31,7 +33,7 @@ public ElevatorIOKrakens() {
                 ElevatorConstants.kInitialHeight * ElevatorConstants.kRotationsToHeightConversion,
                 0.25));
 
-    rightConfig = ElevatorConstants.createKrakenConfig(true);
+    rightConfig = createKrakenConfig(true);
     PhoenixUtil.tryUntilOk(5, () -> right.getConfigurator().apply(rightConfig, 0.25));
     PhoenixUtil.tryUntilOk(
         5,
@@ -41,17 +43,43 @@ public ElevatorIOKrakens() {
                 0.25));
   }
 
+  public TalonFXConfiguration createKrakenConfig(boolean inverted) {
+    var config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.Feedback.SensorToMechanismRatio = ElevatorConstants.kGearReduction;
+    config.TorqueCurrent.PeakForwardTorqueCurrent = ElevatorConstants.kMaxCurrentLimit;
+    config.TorqueCurrent.PeakReverseTorqueCurrent = -ElevatorConstants.kMaxCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimit = ElevatorConstants.kMaxCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.MotorOutput.Inverted =
+        (inverted) ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive;
+
+    // set slot 0 gains
+    config.Slot0 = ElevatorConstants.Gains.toTalonFX();
+
+    // set Motion Magic settings
+    config.MotionMagic = ElevatorConstants.Gains.getMotionMagicConfig();
+
+    config.ClosedLoopGeneral.ContinuousWrap = false;
+    config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    config.SoftwareLimitSwitch.ForwardSoftLimitThreshold =
+        ElevatorConstants.toRotations(ElevatorConstants.kMaxHeight);
+    config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    config.SoftwareLimitSwitch.ReverseSoftLimitThreshold =
+        ElevatorConstants.toRotations(ElevatorConstants.kMinHeight);
+
+    return config;
+  }
+
   @Override
-  public void setPID(LoggedTunablePID slot0pid, LoggedTunablePID slot1pid) {
+  public void setPID(LoggedTunableGains slot0pid) {
     System.out.println("Updating elevator PID");
     leftConfig.Slot0 = slot0pid.toTalonFX();
     rightConfig.Slot0 = slot0pid.toTalonFX();
 
-    leftConfig.Slot1 = slot1pid.toTalonFXS1();
-    rightConfig.Slot1 = slot1pid.toTalonFXS1();
-
-    leftConfig.MotionMagic = slot0pid.toMotionMagic();
-    rightConfig.MotionMagic = slot0pid.toMotionMagic();
+    leftConfig.MotionMagic = slot0pid.getMotionMagicConfig();
+    rightConfig.MotionMagic = slot0pid.getMotionMagicConfig();
 
     PhoenixUtil.tryUntilOk(5, () -> left.getConfigurator().apply(leftConfig, 0.25));
     PhoenixUtil.tryUntilOk(5, () -> right.getConfigurator().apply(rightConfig, 0.25));
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
index 6e5d39f4..8791d621 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
@@ -7,13 +7,13 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 import frc.robot.subsystems.constants.ElevatorConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 
 public class ElevatorIOSim implements ElevatorIO {
   private final DCMotor motor = DCMotor.getKrakenX60Foc(2);
 
-  private PIDController pid = ElevatorConstants.PID_LEVEL_ONE.createPIDController();
-  private ElevatorFeedforward feedforward = ElevatorConstants.PID_LEVEL_TWO.createElevatorFF();
+  private PIDController pid = ElevatorConstants.Gains.createPIDController();
+  private ElevatorFeedforward feedforward = ElevatorConstants.Gains.createElevatorFF();
   private final ElevatorSim sim =
       new ElevatorSim(
           motor,
@@ -57,7 +57,7 @@ public void setTargetHeight(double heightInches) {
   }
 
   @Override
-  public void setPID(LoggedTunablePID pid, LoggedTunablePID pid2) {
+  public void setPID(LoggedTunableGains pid) {
     this.pid = pid.createPIDController();
     feedforward = pid.createElevatorFF();
   }
diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java
index 0e68c4fa..cc3b6d97 100644
--- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java
+++ b/src/main/java/frc/robot/subsystems/pivot/Pivot.java
@@ -35,8 +35,8 @@ public Pivot(PivotIO IO) {
   }
 
   public void periodic() {
-    if (PivotConstants.PID.hasChanged(hashCode())) {
-      this.IO.setPID(PivotConstants.PID);
+    if (PivotConstants.Gains.hasChanged(hashCode())) {
+      this.IO.setPID(PivotConstants.Gains);
     }
 
     IO.updateInputs(inputs);
diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java
index d1d46df9..cadf0344 100644
--- a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java
+++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.pivot;
 
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface PivotIO {
@@ -25,7 +25,7 @@ public default void setVoltage(double volts) {}
   public default void setTargetAngle(double degrees) {}
   ;
 
-  public default void setPID(LoggedTunablePID pid) {}
+  public default void setPID(LoggedTunableGains pid) {}
   ;
 
   public default void setBus(double bus) {}
diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOKrakens.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOKrakens.java
index 65a3454b..95f96896 100644
--- a/src/main/java/frc/robot/subsystems/pivot/PivotIOKrakens.java
+++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOKrakens.java
@@ -5,10 +5,14 @@
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.constants.PivotConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import frc.robot.util.PhoenixUtil;
 
 public class PivotIOKrakens implements PivotIO {
@@ -22,14 +26,48 @@ public class PivotIOKrakens implements PivotIO {
   public PivotIOKrakens() {
     this.motor = new TalonFX(PivotConstants.kCANId);
     this.encoder = new CANcoder(PivotConstants.kEncoderCANId);
-    motorConfig = PivotConstants.createKrakenConfig();
-    encoderConfig = PivotConstants.createCaNcoderConfiguration();
+    motorConfig = createKrakenConfig();
+    encoderConfig = createCaNcoderConfiguration();
     PhoenixUtil.tryUntilOk(5, () -> encoder.getConfigurator().apply(encoderConfig, 0.25));
     PhoenixUtil.tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
     PhoenixUtil.tryUntilOk(
         5, () -> motor.setPosition(encoder.getAbsolutePosition().getValueAsDouble(), 0.25));
   }
 
+  public TalonFXConfiguration createKrakenConfig() {
+    var config = new TalonFXConfiguration();
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.Feedback.FeedbackRemoteSensorID = this.encoder.getDeviceID();
+    config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+    config.Feedback.SensorToMechanismRatio = 1.0;
+    config.Feedback.RotorToSensorRatio = PivotConstants.kGearing;
+    config.TorqueCurrent.PeakForwardTorqueCurrent = PivotConstants.kStallCurrentLimit;
+    config.TorqueCurrent.PeakReverseTorqueCurrent = -PivotConstants.kStallCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimit = PivotConstants.kStatorCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.MotorOutput.Inverted =
+        (PivotConstants.kInverted)
+            ? InvertedValue.CounterClockwise_Positive
+            : InvertedValue.Clockwise_Positive;
+
+    config.Slot0 = PivotConstants.Gains.toTalonFX(GravityTypeValue.Arm_Cosine);
+    config.MotionMagic = PivotConstants.Gains.getMotionMagicConfig();
+    config.ClosedLoopGeneral.ContinuousWrap = false;
+    config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = PivotConstants.kMaxAngleRad;
+    config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = PivotConstants.kMinAngleRad;
+    return config;
+  }
+
+  public CANcoderConfiguration createCaNcoderConfiguration() {
+    var config = new CANcoderConfiguration();
+    config.MagnetSensor.MagnetOffset = PivotConstants.kAbsoluteEncoderOffset;
+    config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
+
+    return config;
+  }
+
   @Override
   public void setVoltage(double volts) {
     motor.setVoltage(volts);
@@ -61,9 +99,9 @@ public void updateInputs(PivotInputs inputs) {
   }
 
   @Override
-  public void setPID(LoggedTunablePID pid) {
+  public void setPID(LoggedTunableGains pid) {
     motorConfig.Slot0 = pid.toTalonFX(GravityTypeValue.Arm_Cosine);
-    motorConfig.MotionMagic = pid.toMotionMagic();
+    motorConfig.MotionMagic = pid.getMotionMagicConfig();
 
     PhoenixUtil.tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
   }
diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java
index 6dcd44de..4797d951 100644
--- a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java
+++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java
@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.subsystems.constants.PivotConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 
 public class PivotIOSim implements PivotIO {
 
@@ -22,13 +22,13 @@ public class PivotIOSim implements PivotIO {
           true,
           PivotConstants.kInitialAngleRad);
 
-  private PIDController pid = PivotConstants.PID.createPIDController();
+  private PIDController pid = PivotConstants.Gains.createPIDController();
 
   public PivotIOSim() {
     pid.enableContinuousInput(0, 2 * Math.PI);
   }
 
-  private ArmFeedforward feedforward = PivotConstants.PID.createArmFF();
+  private ArmFeedforward feedforward = PivotConstants.Gains.createArmFF();
   private double targetAngleDeg = Units.radiansToDegrees(PivotConstants.kInitialAngleRad);
 
   @Override
@@ -61,7 +61,7 @@ public void setTargetAngle(double degrees) {
   }
 
   @Override
-  public void setPID(LoggedTunablePID pid) {
+  public void setPID(LoggedTunableGains pid) {
     this.pid = pid.createPIDController();
     feedforward = pid.createArmFF();
   }
diff --git a/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java b/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java
index e98381fc..6a667428 100644
--- a/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java
+++ b/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java
@@ -38,21 +38,13 @@ public Shoulder(ShoulderIO IO) {
   }
 
   public void periodic() {
-    if (ShoulderConstants.PID.hasChanged(hashCode())) {
-      this.IO.setPID(ShoulderConstants.PID);
+    if (ShoulderConstants.Gains.hasChanged(hashCode())) {
+      this.IO.setPID(ShoulderConstants.Gains);
     }
 
     IO.updateInputs(inputs);
     Logger.processInputs("Shoulder", inputs);
-
-    this.transform =
-        new Transform3d(
-            0.0, 0.0, 0.0, new Rotation3d(0, Units.degreesToRadians(inputs.angleDegrees), 0));
-
-    this.pose =
-        ShoulderConstants.kInitialPose
-            .transformBy(RobotContainer.elevator.transform)
-            .transformBy(this.transform);
+    this.updateTransform();
   }
 
   public void setTargetAngle(double degrees) {
@@ -83,4 +75,15 @@ public void setBus(double bus) {
     Logger.recordOutput("bus", bus);
     IO.setBus(bus);
   }
+
+  private void updateTransform() {
+    this.transform =
+        new Transform3d(
+            0.0, 0.0, 0.0, new Rotation3d(0, Units.degreesToRadians(inputs.angleDegrees), 0));
+
+    this.pose =
+        ShoulderConstants.kInitialPose
+            .transformBy(RobotContainer.elevator.transform)
+            .transformBy(this.transform);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java
index 60e2f81e..c6efebb6 100644
--- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java
+++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.shoulder;
 
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ShoulderIO {
@@ -26,7 +26,7 @@ public default void setVoltage(double volts) {}
   public default void setTargetAngle(double degrees) {}
   ;
 
-  public default void setPID(LoggedTunablePID pid) {}
+  public default void setPID(LoggedTunableGains pid) {}
   ;
 
   public default void setBus(double bus) {}
diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOKrakens.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOKrakens.java
index 53b400d2..2b0f1274 100644
--- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOKrakens.java
+++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOKrakens.java
@@ -5,10 +5,13 @@
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.constants.ShoulderConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 import frc.robot.util.PhoenixUtil;
 
 public class ShoulderIOKrakens implements ShoulderIO {
@@ -22,14 +25,49 @@ public class ShoulderIOKrakens implements ShoulderIO {
   public ShoulderIOKrakens() {
     this.motor = new TalonFX(ShoulderConstants.kCANId);
     this.encoder = new CANcoder(ShoulderConstants.kEncoderCANId);
-    motorConfig = ShoulderConstants.createKrakenConfig();
-    encoderConfig = ShoulderConstants.createCANcoderConfiguration();
+    motorConfig = createKrakenConfig();
+    encoderConfig = createCANcoderConfiguration();
     PhoenixUtil.tryUntilOk(5, () -> this.encoder.getConfigurator().apply(encoderConfig, 0.25));
     PhoenixUtil.tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
     PhoenixUtil.tryUntilOk(
         5, () -> motor.setPosition(encoder.getAbsolutePosition().getValueAsDouble(), 0.25));
   }
 
+  public TalonFXConfiguration createKrakenConfig() {
+    var config = new TalonFXConfiguration();
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.Feedback.FeedbackRemoteSensorID = this.encoder.getDeviceID();
+    config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+    config.Feedback.SensorToMechanismRatio = 1.0;
+    config.Feedback.RotorToSensorRatio = ShoulderConstants.kGearing;
+    config.Feedback.RotorToSensorRatio = 1;
+    config.TorqueCurrent.PeakForwardTorqueCurrent = ShoulderConstants.kStallCurrentLimit;
+    config.TorqueCurrent.PeakReverseTorqueCurrent = -ShoulderConstants.kStallCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimit = ShoulderConstants.kStatorCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.MotorOutput.Inverted =
+        (ShoulderConstants.kInverted)
+            ? InvertedValue.CounterClockwise_Positive
+            : InvertedValue.Clockwise_Positive;
+
+    config.Slot0 = ShoulderConstants.Gains.toTalonFX(GravityTypeValue.Arm_Cosine);
+    config.MotionMagic = ShoulderConstants.Gains.getMotionMagicConfig();
+    config.ClosedLoopGeneral.ContinuousWrap = false;
+    config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ShoulderConstants.kMaxAngleRad;
+    config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ShoulderConstants.kMinAngleRad;
+
+    return config;
+  }
+
+  public static CANcoderConfiguration createCANcoderConfiguration() {
+    var config = new CANcoderConfiguration();
+    config.MagnetSensor.MagnetOffset = ShoulderConstants.kAbsoluteEncoderOffset;
+
+    return config;
+  }
+
   @Override
   public void setVoltage(double volts) {
     motor.setVoltage(volts);
@@ -54,9 +92,9 @@ public void updateInputs(ShoulderInputs inputs) {
   }
 
   @Override
-  public void setPID(LoggedTunablePID pid) {
+  public void setPID(LoggedTunableGains pid) {
     motorConfig.Slot0 = pid.toTalonFX(GravityTypeValue.Arm_Cosine);
-    motorConfig.MotionMagic = pid.toMotionMagic();
+    motorConfig.MotionMagic = pid.getMotionMagicConfig();
 
     PhoenixUtil.tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
   }
diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java
index fefc0f0d..8803dd6d 100644
--- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java
+++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java
@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.subsystems.constants.ShoulderConstants;
-import frc.robot.util.LoggedTunablePID;
+import frc.robot.util.LoggedTunableGains;
 
 public class ShoulderIOSim implements ShoulderIO {
 
@@ -22,13 +22,13 @@ public class ShoulderIOSim implements ShoulderIO {
           true,
           ShoulderConstants.kInitialAngleRad);
 
-  private PIDController pid = ShoulderConstants.PID.createPIDController();
+  private PIDController pid = ShoulderConstants.Gains.createPIDController();
 
   public ShoulderIOSim() {
     pid.enableContinuousInput(0, 2 * Math.PI);
   }
 
-  private ArmFeedforward feedforward = ShoulderConstants.PID.createArmFF();
+  private ArmFeedforward feedforward = ShoulderConstants.Gains.createArmFF();
   private double targetAngleDeg = Units.radiansToDegrees(ShoulderConstants.kInitialAngleRad);
 
   @Override
@@ -59,7 +59,7 @@ public void setTargetAngle(double degrees) {
   }
 
   @Override
-  public void setPID(LoggedTunablePID pid) {
+  public void setPID(LoggedTunableGains pid) {
     this.pid = pid.createPIDController();
     feedforward = pid.createArmFF();
   }
diff --git a/src/main/java/frc/robot/util/ControlGains.java b/src/main/java/frc/robot/util/ControlGains.java
index ede18bcc..ddab2497 100644
--- a/src/main/java/frc/robot/util/ControlGains.java
+++ b/src/main/java/frc/robot/util/ControlGains.java
@@ -24,7 +24,9 @@ public class ControlGains {
 
   @Getter private double kTrapezoidalMaxVelocity;
   @Getter private double kTrapezoidalMaxAcceleration;
-  @Getter private double kMMCruiseVelo;
+  @Getter private double kTrapezoidalMaxJerk;
+  @Getter private double kExponential_kV;
+  @Getter private double kExponential_kA;
 
   public ControlGains() {}
 
@@ -68,14 +70,41 @@ public ControlGains g(double kG) {
     return this;
   }
 
-  public ControlGains trapezoidal(double kMaxVel, double kMaxAccel) {
+  public ControlGains trapezoidal(double kMaxVel, double kMaxAccel, double kMaxJerk) {
     this.kTrapezoidalMaxVelocity = kMaxVel;
     this.kTrapezoidalMaxAcceleration = kMaxAccel;
+    this.kTrapezoidalMaxJerk = kMaxJerk;
     return this;
   }
 
-  public ControlGains mmCruiseVelo(double velo) {
-    this.kMMCruiseVelo = velo;
+  public ControlGains maxTrapezoidalVelocity(double velo) {
+    this.kTrapezoidalMaxVelocity = velo;
+    return this;
+  }
+
+  public ControlGains maxTrapezoidalAcceleration(double accel) {
+    this.kTrapezoidalMaxAcceleration = accel;
+    return this;
+  }
+
+  public ControlGains maxTrapezoidalJerk(double jerk) {
+    this.kTrapezoidalMaxJerk = jerk;
+    return this;
+  }
+
+  public ControlGains exponential(double kV, double kA) {
+    this.kExponential_kV = kV;
+    this.kExponential_kA = kA;
+    return this;
+  }
+
+  public ControlGains expo_kV(double kV) {
+    this.kExponential_kV = kV;
+    return this;
+  }
+
+  public ControlGains expo_kA(double kA) {
+    this.kExponential_kA = kA;
     return this;
   }
 
@@ -106,9 +135,9 @@ public ElevatorFeedforward createElevatorFF() {
 
   public MotionMagicConfigs createMMConfigs() {
     var configs = new MotionMagicConfigs();
-    configs.MotionMagicCruiseVelocity = this.kMMCruiseVelo;
-    configs.MotionMagicExpo_kA = this.kA;
-    configs.MotionMagicExpo_kV = this.kV;
+    configs.MotionMagicCruiseVelocity = this.kTrapezoidalMaxVelocity;
+    configs.MotionMagicExpo_kA = this.kExponential_kA;
+    configs.MotionMagicExpo_kV = this.kExponential_kV;
 
     return configs;
   }
diff --git a/src/main/java/frc/robot/util/LoggedTunablePID.java b/src/main/java/frc/robot/util/LoggedTunableGains.java
similarity index 80%
rename from src/main/java/frc/robot/util/LoggedTunablePID.java
rename to src/main/java/frc/robot/util/LoggedTunableGains.java
index 674da5a0..a1bc4068 100644
--- a/src/main/java/frc/robot/util/LoggedTunablePID.java
+++ b/src/main/java/frc/robot/util/LoggedTunableGains.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc.robot.Constants;
 
-public class LoggedTunablePID {
+public class LoggedTunableGains {
   public final LoggedTunableNumber kP;
   public final LoggedTunableNumber kI;
   public final LoggedTunableNumber kD;
@@ -24,13 +24,10 @@ public class LoggedTunablePID {
   public final LoggedTunableNumber MotionMagicCruiseVelocity;
   public final LoggedTunableNumber MotionMagicAcceleration;
   public final LoggedTunableNumber MotionMagicJerk;
+  public final LoggedTunableNumber MotionMagicExpo_kV;
+  public final LoggedTunableNumber MotionMagicExpo_kA;
 
-  public LoggedTunablePID(String subsystem, ControlGains PID) {
-    this(subsystem, PID, 0.0, 0.0);
-  }
-
-  public LoggedTunablePID(
-      String subsystem, ControlGains PID, double MotionMagicAcceleration, double MotionMagicJerk) {
+  public LoggedTunableGains(String subsystem, ControlGains PID) {
     kP = new LoggedTunableNumber(subsystem + "/PID/P", PID.getKP());
     kI = new LoggedTunableNumber(subsystem + "/PID/I", PID.getKI());
     kD = new LoggedTunableNumber(subsystem + "/PID/D", PID.getKD());
@@ -39,10 +36,18 @@ public LoggedTunablePID(
     kV = new LoggedTunableNumber(subsystem + "/PID/V", PID.getKV());
     kA = new LoggedTunableNumber(subsystem + "/PID/A", PID.getKA());
     MotionMagicCruiseVelocity =
-        new LoggedTunableNumber(subsystem + "/PID/CruiseVelocity", PID.getKMMCruiseVelo());
+        new LoggedTunableNumber(
+            subsystem + "/PID/CruiseVelocity", PID.getKTrapezoidalMaxVelocity());
     this.MotionMagicAcceleration =
-        new LoggedTunableNumber(subsystem + "/PID/Acceleration", MotionMagicAcceleration);
-    this.MotionMagicJerk = new LoggedTunableNumber(subsystem + "/PID/Jerk", MotionMagicJerk);
+        new LoggedTunableNumber(
+            subsystem + "/PID/Acceleration", PID.getKTrapezoidalMaxAcceleration());
+    this.MotionMagicJerk =
+        new LoggedTunableNumber(subsystem + "/PID/Jerk", PID.getKTrapezoidalMaxJerk());
+
+    this.MotionMagicExpo_kV =
+        new LoggedTunableNumber(subsystem + "/PID/Expo_kV", PID.getKExponential_kV());
+    this.MotionMagicExpo_kA =
+        new LoggedTunableNumber(subsystem + "/PID/Expo_kA", PID.getKExponential_kA());
   }
 
   public boolean hasChanged(int id) {
@@ -58,7 +63,9 @@ public boolean hasChanged(int id) {
         || kA.hasChanged(id)
         || MotionMagicCruiseVelocity.hasChanged(id)
         || MotionMagicAcceleration.hasChanged(id)
-        || MotionMagicJerk.hasChanged(id);
+        || MotionMagicJerk.hasChanged(id)
+        || MotionMagicExpo_kV.hasChanged(id)
+        || MotionMagicExpo_kA.hasChanged(id);
   }
 
   public double getKP() {
@@ -130,12 +137,14 @@ public Slot1Configs toTalonFXS1(GravityTypeValue gravityType) {
     return slot1Configs;
   }
 
-  public MotionMagicConfigs toMotionMagic() {
+  public MotionMagicConfigs getMotionMagicConfig() {
     var motionMagicConfigs = new MotionMagicConfigs();
 
     motionMagicConfigs.MotionMagicCruiseVelocity = MotionMagicCruiseVelocity.getAsDouble();
     motionMagicConfigs.MotionMagicAcceleration = MotionMagicAcceleration.getAsDouble();
     motionMagicConfigs.MotionMagicJerk = MotionMagicJerk.getAsDouble();
+    motionMagicConfigs.MotionMagicExpo_kV = MotionMagicExpo_kV.getAsDouble();
+    motionMagicConfigs.MotionMagicExpo_kA = MotionMagicExpo_kA.getAsDouble();
 
     return motionMagicConfigs;
   }


Reviews:
user: login: unbun
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-03-02T22:45:24Z
user: login: tytremblay
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-02T22:55:53Z

Review comments:
diff_hunk: @@ -72,48 +68,17 @@ public class PivotConstants {
               .s(kS)
               .v(kV)
               .a(kA)
-              // Motion Magic
-              .mmCruiseVelo(1),
-          80,
-          1600);
+              .maxTrapezoidalVelocity(kTrapezoidalMaxVelocity)
+              .maxTrapezoidalAcceleration(kTrapezoidalMaxAcceleration)
+              .maxTrapezoidalJerk(kTrapezoidalMaxJerk)
+              .expo_kV(kExponential_kV)
+              .expo_kA(kExponential_kA));
path: src/main/java/frc/robot/subsystems/constants/PivotConstants.java
user: login: unbun
body: it looks like this keeps MotionMagicExpo configs in place, so is the plan to keep working with expo? just wanted to ask since we briefly talked about simplifying things with just trapezoidal profiles (same question for ShoulderConstants.java)
created_at: 2025-03-02T22:45:08Z
author_association: CONTRIBUTOR
diff_hunk: @@ -72,48 +68,17 @@ public class PivotConstants {
               .s(kS)
               .v(kV)
               .a(kA)
-              // Motion Magic
-              .mmCruiseVelo(1),
-          80,
-          1600);
+              .maxTrapezoidalVelocity(kTrapezoidalMaxVelocity)
+              .maxTrapezoidalAcceleration(kTrapezoidalMaxAcceleration)
+              .maxTrapezoidalJerk(kTrapezoidalMaxJerk)
+              .expo_kV(kExponential_kV)
+              .expo_kA(kExponential_kA));
path: src/main/java/frc/robot/subsystems/constants/PivotConstants.java
user: login: tytremblay
body: after looking at it, they're both complicated and require tuning, so i figure we can pick whichever.  if we choose to go with trapezoidal, then these gains will just be ignored.
created_at: 2025-03-02T22:55:53Z
author_association: CONTRIBUTOR

PR comments:
user: login: Sid220
created_at: 2025-03-02T22:22:29Z
body: @tytremblay see my recent changes in drive-tuning branch. Can you merge into that instead or merge drive-tuning into this? Also: adding/subtracting 360 degrees to a set point did seem to change which direction it would go, at least in sim.
author_association: COLLABORATOR
user: login: tytremblay
created_at: 2025-03-02T22:28:01Z
body: > @tytremblay see my recent changes in drive-tuning branch. Can you merge into that instead or merge drive-tuning into this? Also: adding/subtracting 360 degrees to a set point did seem to change which direction it would go, at least in sim.

in sim, it uses a different controller to do the trapezoidal motion, right?  these changes disable Continuous wrap on the kraken, which makes 90 deg (straight up) and -180 degrees (straight up) different positions.  If we don't disable continuous wrap, those are recognized to be equivalent and the talonFX takes the shorter path.

author_association: CONTRIBUTOR