Tune pid without redeploying

simgui-ds.json

@@ -90,6 +90,10 @@
     }
   ],
   "robotJoysticks": [
+    {
+      "useGamepad": true
+    },
+    {},
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true

src/main/java/frc/robot/RobotContainer.java

@@ -28,11 +28,14 @@
 import monologue.Logged;
 import frc.robot.util.Constants.NTConstants;
 import monologue.Annotations.Log;
+import frc.robot.util.PIDNotConstants;
+import frc.robot.util.PIDTunerCommands;
 
 public class RobotContainer implements Logged {
 
     private final PatriBoxController driver;
     private final PatriBoxController operator;
+    private final PatriBoxController PIDTunerController;
 
     private Swerve swerve;
     private final Intake intake;
@@ -49,6 +52,7 @@ public class RobotContainer implements Logged {
     private Elevator elevator;
     private ShooterCalc shooterCalc;
     private PieceControl pieceControl;
+    private PIDTunerCommands PIDTuner;
 
     @Log.NT
     public static Pose3d[] components3d = new Pose3d[5];
@@ -60,6 +64,7 @@ public RobotContainer() {
 
         driver = new PatriBoxController(OIConstants.DRIVER_CONTROLLER_PORT, OIConstants.DRIVER_DEADBAND);
         operator = new PatriBoxController(OIConstants.OPERATOR_CONTROLLER_PORT, OIConstants.OPERATOR_DEADBAND);
+        PIDTunerController = new PatriBoxController(OIConstants.PID_TUNER_CONTROLLER_PORT, OIConstants.PID_TUNER_DEADBAND);
         DriverStation.silenceJoystickConnectionWarning(true);
 
         limelight = new Limelight();
@@ -77,6 +82,13 @@ public RobotContainer() {
 
         shooterCalc = new ShooterCalc(shooter, pivot);
 
+        PIDTuner = new PIDTunerCommands(new PIDNotConstants[] {
+            pivot.getPIDNotConstants(),
+            shooter.getPIDNotConstants(),
+            elevator.getPIDNotConstants(),
+            climb.getPidNotConstants()
+        });
+
         pieceControl = new PieceControl(
             intake,
             triggerWheel,
@@ -119,8 +131,20 @@ public RobotContainer() {
     private void configureButtonBindings() {
         configureDriverBindings(driver);
         configureOperatorBindings(operator);
+        configurePIDTunerBindings(PIDTunerController);
     }
 
+    private void configurePIDTunerBindings(PatriBoxController controller) {
+        controller.povRight().onTrue(PIDTuner.incrementSubsystemCommand());
+        controller.povLeft().onTrue(PIDTuner.decreaseSubsystemCommand());
+        controller.rightBumper().onTrue(PIDTuner.PIDIncrementCommand());
+        controller.leftBumper().onTrue(PIDTuner.PIDDecreaseCommand());
+        controller.povUp().onTrue(PIDTuner.increaseCurrentPIDCommand(1000));
+        controller.povDown().onTrue(PIDTuner.decreaseCurrentPIDCommand(1000));
+        controller.a().onTrue(PIDTuner.logCommand());
+    }
+
+
     private void configureOperatorBindings(PatriBoxController controller) {
 
         controller.povUp().toggleOnTrue(climb.povUpCommand(swerve::getPose));

src/main/java/frc/robot/subsystems/Climb.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotContainer;
 import frc.robot.util.Neo;
+import frc.robot.util.PIDNotConstants;
 import frc.robot.util.PoseCalculations;
 import frc.robot.util.Constants.ClimbConstants;
 import frc.robot.util.Constants.NTConstants;
@@ -20,12 +21,14 @@ public class Climb extends SubsystemBase implements Logged {
 
     private final Neo leftMotor;
     private final Neo rightMotor;
+    private final PIDNotConstants climbPID;
 
     public Climb() {
         leftMotor = new Neo(ClimbConstants.LEFT_CLIMB_CAN_ID);
         rightMotor = new Neo(ClimbConstants.RIGHT_CLIMB_CAN_ID);
 
         configureMotors();
+        climbPID = new PIDNotConstants(leftMotor.getPID(), leftMotor.getPIDController());
     }
 
     private void configureMotors() {
@@ -52,6 +55,13 @@ public void periodic() {
             0, 0, rightMotor.getPosition(),
             new Rotation3d()
         );
+        climbPID.updatePID();
+        rightMotor.setPID(climbPID.getPID());
+    }
+
+
+    public PIDNotConstants getPidNotConstants() {
+        return this.climbPID;
     }
 
     public void setPosition(Pair<Double, Double> positionPair) {

src/main/java/frc/robot/subsystems/Swerve.java

@@ -32,6 +32,7 @@
 import frc.robot.DriverUI;
 import frc.robot.commands.Drive;
 import frc.robot.util.MAXSwerveModule;
+import frc.robot.util.PIDNotConstants;
 import frc.robot.util.Constants.AutoConstants;
 import frc.robot.util.Constants.DriveConstants;
 import frc.robot.util.Constants.FieldConstants;
@@ -43,7 +44,6 @@ public class Swerve extends SubsystemBase implements Logged {
     public static double twistScalar = 4;
 
     private double speedMultiplier = 1;
-
     private final MAXSwerveModule frontLeft = new MAXSwerveModule(
             DriveConstants.FRONT_LEFT_DRIVING_CAN_ID,
             DriveConstants.FRONT_LEFT_TURNING_CAN_ID,

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -12,17 +12,19 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotContainer;
 import frc.robot.util.Neo;
+import frc.robot.util.PIDNotConstants;
 import frc.robot.util.Constants.NTConstants;
 import frc.robot.util.Constants.TrapConstants;
 
 public class Elevator extends SubsystemBase {
     private final Neo elevator;
-
+    private final PIDNotConstants elevatorPID;
 
     /** Creates a new Elevator. */
     public Elevator() {
         elevator = new Neo(TrapConstants.LEFT_ELEVATOR_CAN_ID);
         configMotors();
+        elevatorPID = new PIDNotConstants(elevator.getPID(), elevator.getPIDController());
     }
 
     public void configMotors() {
@@ -43,6 +45,7 @@ public void periodic() {
             0, 0, elevator.getPosition(),
             new Rotation3d()
         );
+        elevatorPID.updatePID();
     }
 
     public double getPosition() {
@@ -92,5 +95,8 @@ public Command toTopCommand() {
     public Command stop() {
         return runOnce(() -> elevator.stopMotor());
     }
+    public PIDNotConstants getPIDNotConstants() {
+        return this.elevatorPID;
+    }
 
 }

src/main/java/frc/robot/subsystems/shooter/Pivot.java

@@ -12,14 +12,18 @@
 import frc.robot.util.Constants.ShooterConstants;
 import frc.robot.util.Neo.TelemetryPreference;
 import monologue.Logged;
+import com.pathplanner.lib.util.PIDConstants;
+import frc.robot.util.PIDNotConstants;
 
 public class Pivot extends SubsystemBase implements Logged {
 	private Neo pivot;
+    private PIDNotConstants pivotPID;
 
 	public Pivot() {
 		this.pivot = new Neo(ShooterConstants.SHOOTER_PIVOT_CAN_ID, true);
 
 		configMotor();
+        pivotPID = new PIDNotConstants(pivot.getPID(), pivot.getPIDController());
 	}
 
 	public void configMotor() {
@@ -35,6 +39,7 @@ public void configMotor() {
 				ShooterConstants.PIVOT_MAX_OUTPUT);
 
 		// sets brake mode
+
 		pivot.setBrakeMode();
 	}
 
@@ -46,6 +51,7 @@ public void periodic() {
 			NTConstants.PIVOT_OFFSET_METERS.getZ(),
 			new Rotation3d(0, Units.degreesToRadians(getAngle()), 0)
 		);
+        pivotPID.updatePID();
 	}
 
 	/**
@@ -68,7 +74,9 @@ public void setAngle(double angle) {
 			new Rotation3d(0, Units.degreesToRadians(angle), 0)
 		);
 	}
-
+    public PIDNotConstants getPIDNotConstants() {
+        return this.pivotPID;
+    }
 	/**
 	 * The function takes an angle in degrees and returns a command that sets
 	 * the pivot to the angle converted to a position
@@ -111,4 +119,5 @@ public double getAngle() {
 	public Command stop() {
 		return runOnce(() -> pivot.stopMotor());
 	}
+
 }

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -6,19 +6,21 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.util.Neo;
 import frc.robot.util.Constants.ShooterConstants;
+import frc.robot.util.PIDNotConstants;
 
 public class Shooter extends SubsystemBase {
     /** Creates a new shooter. */
     private final Neo motorLeft;
     private final Neo motorRight;
 
+    public final PIDNotConstants shooterPID;
     public Shooter() {
 
         motorLeft = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID);
         motorRight = new Neo(ShooterConstants.RIGHT_SHOOTER_CAN_ID);
 
         configMotors();
-
+        shooterPID = new PIDNotConstants(motorLeft.getPID(), motorLeft.getPIDController());
     }
 
     public void configMotors() {
@@ -42,8 +44,12 @@ public void configMotors() {
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
+        shooterPID.updatePID();
+        motorRight.setPID(shooterPID.getPID());
     }
 
+
+
     /**
      * The function sets both of the motors to a targetVelocity that is
      * the speed provided
@@ -88,4 +94,8 @@ public Pair<Double, Double> getSpeed() {
     public Command stop() {
         return Commands.runOnce(() -> motorLeft.stopMotor());
     }
+
+    public PIDNotConstants getPIDNotConstants() {
+        return this.shooterPID;
+    }
 }

src/main/java/frc/robot/util/Constants.java

@@ -27,6 +27,8 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Robot;
+import monologue.Logged;
+import monologue.Annotations.Log;
 
 /**
  * Welcome to the home of the many many variables :D
@@ -96,6 +98,16 @@ public static final class DriveConstants {
         public static final boolean GYRO_REVERSED = true;
     }
 
+
+    public static final class tuningConstants implements Logged {
+        public final int DRIVE_INDEX = 0;
+        public final int PIVOT_INDEX = 1;
+        public final int SHOOTER_INDEX = 2;
+        public final int ELEVATOR_INDEX = 3;
+        public final int CLIMB_INDEX = 4;
+
+    }
+
     public static final class ShooterConstants {
         public static final int LEFT_SHOOTER_CAN_ID = 11;
         public static final int RIGHT_SHOOTER_CAN_ID = 12;
@@ -108,6 +120,7 @@ public static final class ShooterConstants {
         public static final double SHOOTER_I = 0;
         public static final double SHOOTER_D = 0;
 
+
         // TODO: tune pid further
         public static final double PIVOT_P = 0.2;
         public static final double PIVOT_I = 0;
@@ -340,9 +353,11 @@ public static final class OIConstants {
 
         public static final int DRIVER_CONTROLLER_PORT = 0;
         public static final int OPERATOR_CONTROLLER_PORT = 1;
+        public static final int PID_TUNER_CONTROLLER_PORT = 2;
 
         public static final double DRIVER_DEADBAND = 0.15;
         public static final double OPERATOR_DEADBAND = 0.15;
+        public static final double PID_TUNER_DEADBAND = 0.15;
 
         // See https://www.desmos.com/calculator/e07raajzh5
         // And

src/main/java/frc/robot/util/Neo.java

@@ -448,6 +448,10 @@ public double getD() {
         return pidController.getD();
     }
 
+    public PIDConstants getPID() {
+        return new PIDConstants(pidController.getP(), pidController.getI(), pidController.getD());
+    }
+
     /**
      * Gets the I-Zone constant for PID controller.
      * The I-Zone is the zone at which the integral