From 3c00a30a2e92267706660ccdedf3f0dbb9153b41 Mon Sep 17 00:00:00 2001 From: RudyG252 <122180943+RudyG252@users.noreply.github.com> Date: Tue, 6 Feb 2024 11:28:33 -0800 Subject: [PATCH 1/5] Basic functionality for shooter. I need to add to different subsystems and make increase and decrease values better maybe? --- simgui-ds.json | 3 +- src/main/java/frc/robot/RobotContainer.java | 20 ++++ .../java/frc/robot/subsystems/Swerve.java | 2 +- .../frc/robot/subsystems/shooter/Pivot.java | 2 + .../frc/robot/subsystems/shooter/Shooter.java | 12 +- src/main/java/frc/robot/util/Constants.java | 15 +++ src/main/java/frc/robot/util/Neo.java | 4 + .../java/frc/robot/util/PIDNotConstants.java | 107 ++++++++++++++++++ .../java/frc/robot/util/PIDTunerCommands.java | 73 ++++++++++++ 9 files changed, 234 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/util/PIDNotConstants.java create mode 100644 src/main/java/frc/robot/util/PIDTunerCommands.java diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd3..49cf3aae 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "78696e70757401000000000000000000" } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5a7d9d01..f058f956 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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,10 @@ public RobotContainer() { shooterCalc = new ShooterCalc(shooter, pivot); + PIDTuner = new PIDTunerCommands(new PIDNotConstants[] { + shooter.getPIDNotConstants() + }); + pieceControl = new PieceControl( intake, triggerWheel, @@ -119,8 +128,19 @@ 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(.05)); + controller.povDown().onTrue(PIDTuner.decreaseCurrentPIDCommand(.05)); + } + + private void configureOperatorBindings(PatriBoxController controller) { controller.povUp().toggleOnTrue(climb.povUpCommand(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 525a61c9..e09e5995 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/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, diff --git a/src/main/java/frc/robot/subsystems/shooter/Pivot.java b/src/main/java/frc/robot/subsystems/shooter/Pivot.java index 1713f937..1a2d48d0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Pivot.java +++ b/src/main/java/frc/robot/subsystems/shooter/Pivot.java @@ -12,6 +12,7 @@ import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.Neo.TelemetryPreference; import monologue.Logged; +import com.pathplanner.lib.util.PIDConstants; public class Pivot extends SubsystemBase implements Logged { private Neo pivot; @@ -111,4 +112,5 @@ public double getAngle() { public Command stop() { return runOnce(() -> pivot.stopMotor()); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 325da3b9..9bebd1f7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/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 getSpeed() { public Command stop() { return Commands.runOnce(() -> motorLeft.stopMotor()); } + + public PIDNotConstants getPIDNotConstants() { + return this.shooterPID; + } } diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 9c51f7e0..0cd9b35c 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/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 diff --git a/src/main/java/frc/robot/util/Neo.java b/src/main/java/frc/robot/util/Neo.java index 230f0ec8..d001ed46 100644 --- a/src/main/java/frc/robot/util/Neo.java +++ b/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 diff --git a/src/main/java/frc/robot/util/PIDNotConstants.java b/src/main/java/frc/robot/util/PIDNotConstants.java new file mode 100644 index 00000000..0bba2664 --- /dev/null +++ b/src/main/java/frc/robot/util/PIDNotConstants.java @@ -0,0 +1,107 @@ +package frc.robot.util; +import com.revrobotics.SparkPIDController; +import com.pathplanner.lib.util.PIDConstants; +public class PIDNotConstants { + /** P */ + public double kP; + /** I */ + public double kI; + /** D */ + public double kD; + /** Integral range */ + public final double iZone; + + private final SparkPIDController PIDController; + + + + + public PIDNotConstants(double kP, double kI, double kD, double iZone, SparkPIDController PIDController) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.iZone = iZone; + this.PIDController = PIDController; + this.updatePID(); + } + /** + * Create a new PIDConstants object + * + * @param kP P + * @param kI I + * @param kD D + */ + public PIDNotConstants(double kP, double kI, double kD, SparkPIDController PIDController) { + this(kP, kI, kD, 1.0, PIDController); + } + + /** + * Create a new PIDConstants object + * + * @param kP P + * @param kD D + */ + public PIDNotConstants(double kP, double kD, SparkPIDController PIDController) { + this(kP, 0, kD, PIDController); + } + + public void updatePID() { + this.PIDController.setP(this.kP); + this.PIDController.setI(this.kI); + this.PIDController.setD(this.kI); + } + /** + * Create a new PIDConstants object + * + * @param kP P + */ + public PIDNotConstants(double kP, SparkPIDController PIDController) { + this(kP, 0, 0, PIDController); + } + + public PIDNotConstants(PIDConstants PID, SparkPIDController PIDController) { + this(PID.kP, PID.kD, PID.kI, 1.0, PIDController); + } + + public PIDConstants getPID() { + return new PIDConstants(this.kP, this.kD, this.kI); + } + + public void incrementPBy(double value) { + kP += value; + } + + public void incrementIBy(double value) { + kI += value; + } + + public void incrementDBy(double value) { + kD += value; + } + + public void setP(double value) { + kP = value; + } + + public void setI(double value) { + kI = value; + } + + public void setD(double value) { + kD = value; + } + + public double getP() { + return this.kP; + } + + public double getI() { + return this.kI; + } + + public double getD() { + return this.kD; + } + + +} diff --git a/src/main/java/frc/robot/util/PIDTunerCommands.java b/src/main/java/frc/robot/util/PIDTunerCommands.java new file mode 100644 index 00000000..06c2e1d3 --- /dev/null +++ b/src/main/java/frc/robot/util/PIDTunerCommands.java @@ -0,0 +1,73 @@ +package frc.robot.util; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +public class PIDTunerCommands { + + private PIDNotConstants[] subsystems; + private Command[] PIDChangeArray; + private int subsystemIndex; + private int PIDIndex; + + public PIDTunerCommands(PIDNotConstants[] subsystems) { + this.subsystems = subsystems; + this.subsystemIndex = 0; + this.PIDIndex = 0; + } + + public PIDNotConstants getCurrentSubsystem() { + return this.subsystems[subsystemIndex]; + } + + public void increment() { + if (subsystemIndex < subsystems.length) { + this.subsystemIndex++; + } + } + public void PIDIncrement() { + if (PIDIndex < PIDChangeArray.length) { + this.PIDIndex++; + } + } + + public void PIDDecrease() { + if (PIDIndex > 0) { + this.PIDIndex--; + } + } + + public void decrease() { + if (subsystemIndex > 0) { + this.subsystemIndex--; + } + } + public void increasePID(double value) { + switch (PIDIndex) { + case 0: + subsystems[subsystemIndex].setP(value); + case 1: + subsystems[subsystemIndex].setI(.01 * value); + case 2: + subsystems[subsystemIndex].setD(value); + } + } + + + public Command incrementSubsystemCommand() { + return Commands.runOnce(() -> increment()); + } + public Command decreaseSubsystemCommand() { + return Commands.runOnce(() -> decrease()); + } + public Command increaseCurrentPIDCommand(double value) { + return Commands.runOnce(() -> this.increasePID(value)); + } + public Command decreaseCurrentPIDCommand(double value) { + return Commands.runOnce(() -> this.increasePID(-value)); + } + public Command PIDIncrementCommand() { + return Commands.runOnce(() -> PIDIncrement()); + } + public Command PIDDecreaseCommand() { + return Commands.runOnce(() -> PIDIncrement()); + } +} From 941cb40e125cca8824de71f36a7c93954c1afbf6 Mon Sep 17 00:00:00 2001 From: RudyG252 <122180943+RudyG252@users.noreply.github.com> Date: Tue, 6 Feb 2024 11:42:37 -0800 Subject: [PATCH 2/5] Added PIDNotConstants to subystems. Need to add them into robot container then test in sim but english is ending. --- src/main/java/frc/robot/subsystems/Climb.java | 5 +++++ .../frc/robot/subsystems/elevator/Elevator.java | 5 ++++- .../java/frc/robot/subsystems/shooter/Pivot.java | 9 ++++++++- src/main/java/frc/robot/util/PIDNotConstants.java | 14 ++++++++++---- 4 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java index 923c3f2c..ff2476e3 100644 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ b/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,8 @@ public void periodic() { 0, 0, rightMotor.getPosition(), new Rotation3d() ); + climbPID.updatePID(); + rightMotor.setPID(climbPID.getPID()); } public void setPosition(Pair positionPair) { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 9b021dc7..d9f3695b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Pivot.java b/src/main/java/frc/robot/subsystems/shooter/Pivot.java index 1a2d48d0..d2887ca4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Pivot.java +++ b/src/main/java/frc/robot/subsystems/shooter/Pivot.java @@ -13,14 +13,17 @@ 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() { @@ -36,6 +39,7 @@ public void configMotor() { ShooterConstants.PIVOT_MAX_OUTPUT); // sets brake mode + pivot.setBrakeMode(); } @@ -47,6 +51,7 @@ public void periodic() { NTConstants.PIVOT_OFFSET_METERS.getZ(), new Rotation3d(0, Units.degreesToRadians(getAngle()), 0) ); + pivotPID.updatePID(); } /** @@ -69,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 diff --git a/src/main/java/frc/robot/util/PIDNotConstants.java b/src/main/java/frc/robot/util/PIDNotConstants.java index 0bba2664..c097b251 100644 --- a/src/main/java/frc/robot/util/PIDNotConstants.java +++ b/src/main/java/frc/robot/util/PIDNotConstants.java @@ -46,9 +46,15 @@ public PIDNotConstants(double kP, double kD, SparkPIDController PIDController) { } public void updatePID() { - this.PIDController.setP(this.kP); - this.PIDController.setI(this.kI); - this.PIDController.setD(this.kI); + if (PIDController.getP() != this.kP) { + this.PIDController.setP(this.kP); + } + if (PIDController.getI() != this.kI) { + this.PIDController.setI(this.kI); + } + if (PIDController.getD() != this.kD) { + this.PIDController.setD(this.kD); + } } /** * Create a new PIDConstants object @@ -58,7 +64,7 @@ public void updatePID() { public PIDNotConstants(double kP, SparkPIDController PIDController) { this(kP, 0, 0, PIDController); } - + public PIDNotConstants(PIDConstants PID, SparkPIDController PIDController) { this(PID.kP, PID.kD, PID.kI, 1.0, PIDController); } From 5d1b95cfb4b4bb2b608c6a8bb63a2e6a96c497b8 Mon Sep 17 00:00:00 2001 From: RudyG252 <122180943+RudyG252@users.noreply.github.com> Date: Tue, 6 Feb 2024 13:36:56 -0800 Subject: [PATCH 3/5] added everything to robot container :D --- src/main/java/frc/robot/RobotContainer.java | 7 +++++-- src/main/java/frc/robot/subsystems/Climb.java | 5 +++++ src/main/java/frc/robot/subsystems/elevator/Elevator.java | 3 +++ 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f058f956..f8c3cfc6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,7 +83,10 @@ public RobotContainer() { shooterCalc = new ShooterCalc(shooter, pivot); PIDTuner = new PIDTunerCommands(new PIDNotConstants[] { - shooter.getPIDNotConstants() + pivot.getPIDNotConstants(), + shooter.getPIDNotConstants(), + elevator.getPIDNotConstants(), + climb.getPidNotConstants() }); pieceControl = new PieceControl( @@ -139,7 +142,7 @@ private void configurePIDTunerBindings(PatriBoxController controller) { controller.povUp().onTrue(PIDTuner.increaseCurrentPIDCommand(.05)); controller.povDown().onTrue(PIDTuner.decreaseCurrentPIDCommand(.05)); } - + private void configureOperatorBindings(PatriBoxController controller) { diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java index ff2476e3..1a5a168f 100644 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ b/src/main/java/frc/robot/subsystems/Climb.java @@ -59,6 +59,11 @@ public void periodic() { rightMotor.setPID(climbPID.getPID()); } + + public PIDNotConstants getPidNotConstants() { + return this.climbPID; + } + public void setPosition(Pair positionPair) { setPosition(positionPair.getFirst(), positionPair.getSecond()); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index d9f3695b..ffb9606e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -95,5 +95,8 @@ public Command toTopCommand() { public Command stop() { return runOnce(() -> elevator.stopMotor()); } + public PIDNotConstants getPIDNotConstants() { + return this.elevatorPID; + } } From 1389aa8cb9d418344f79a592baa050f8f8a355b8 Mon Sep 17 00:00:00 2001 From: RudyG252 <122180943+RudyG252@users.noreply.github.com> Date: Tue, 6 Feb 2024 19:59:16 -0800 Subject: [PATCH 4/5] fixed bugs. It works now but getting a lot of loop overruns for some reason, which might be a problem or might be my computer tweaking. --- simgui-ds.json | 7 ++- src/main/java/frc/robot/RobotContainer.java | 5 ++- .../java/frc/robot/util/PIDNotConstants.java | 20 +++------ .../java/frc/robot/util/PIDTunerCommands.java | 45 +++++++++++++++---- 4 files changed, 52 insertions(+), 25 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 49cf3aae..e3d7f018 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,12 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000" + "useGamepad": true + }, + {}, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f8c3cfc6..aa80dc6d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -139,8 +139,9 @@ private void configurePIDTunerBindings(PatriBoxController controller) { controller.povLeft().onTrue(PIDTuner.decreaseSubsystemCommand()); controller.rightBumper().onTrue(PIDTuner.PIDIncrementCommand()); controller.leftBumper().onTrue(PIDTuner.PIDDecreaseCommand()); - controller.povUp().onTrue(PIDTuner.increaseCurrentPIDCommand(.05)); - controller.povDown().onTrue(PIDTuner.decreaseCurrentPIDCommand(.05)); + controller.povUp().onTrue(PIDTuner.increaseCurrentPIDCommand(1000)); + controller.povDown().onTrue(PIDTuner.decreaseCurrentPIDCommand(1000)); + controller.a().onTrue(PIDTuner.logCommand()); } diff --git a/src/main/java/frc/robot/util/PIDNotConstants.java b/src/main/java/frc/robot/util/PIDNotConstants.java index c097b251..6553b492 100644 --- a/src/main/java/frc/robot/util/PIDNotConstants.java +++ b/src/main/java/frc/robot/util/PIDNotConstants.java @@ -56,6 +56,10 @@ public void updatePID() { this.PIDController.setD(this.kD); } } + + public String toString() { + return "P: " + this.kP + "\nI: " + this.kI + "\nD: " + this.kD; + } /** * Create a new PIDConstants object * @@ -66,23 +70,11 @@ public PIDNotConstants(double kP, SparkPIDController PIDController) { } public PIDNotConstants(PIDConstants PID, SparkPIDController PIDController) { - this(PID.kP, PID.kD, PID.kI, 1.0, PIDController); + this(PID.kP, PID.kI, PID.kD, 1.0, PIDController); } public PIDConstants getPID() { - return new PIDConstants(this.kP, this.kD, this.kI); - } - - public void incrementPBy(double value) { - kP += value; - } - - public void incrementIBy(double value) { - kI += value; - } - - public void incrementDBy(double value) { - kD += value; + return new PIDConstants(this.kP, this.kI, this.kD); } public void setP(double value) { diff --git a/src/main/java/frc/robot/util/PIDTunerCommands.java b/src/main/java/frc/robot/util/PIDTunerCommands.java index 06c2e1d3..0883b67d 100644 --- a/src/main/java/frc/robot/util/PIDTunerCommands.java +++ b/src/main/java/frc/robot/util/PIDTunerCommands.java @@ -4,7 +4,6 @@ public class PIDTunerCommands { private PIDNotConstants[] subsystems; - private Command[] PIDChangeArray; private int subsystemIndex; private int PIDIndex; @@ -19,12 +18,13 @@ public PIDNotConstants getCurrentSubsystem() { } public void increment() { - if (subsystemIndex < subsystems.length) { + if (subsystemIndex < subsystems.length - 1) { this.subsystemIndex++; } } + public void PIDIncrement() { - if (PIDIndex < PIDChangeArray.length) { + if (PIDIndex < 2) { this.PIDIndex++; } } @@ -35,20 +35,45 @@ public void PIDDecrease() { } } + public void log() { + System.out.println("Subsystem: " + subsystemToString(subsystemIndex)); + System.out.println(subsystems[subsystemIndex].toString()); + System.out.println("PID Index: " + this.PIDIndex); + } + + + public String subsystemToString(int index) { + switch (index) { + case 0: + return "Pivot"; + case 1: + return "Shooter"; + case 2: + return "Elevator"; + case 3: + return "Climb"; + } + return ""; + } + public void decrease() { if (subsystemIndex > 0) { this.subsystemIndex--; } } public void increasePID(double value) { - switch (PIDIndex) { + switch (this.PIDIndex) { case 0: - subsystems[subsystemIndex].setP(value); + subsystems[subsystemIndex].setP(subsystems[subsystemIndex].getP() + value); + break; case 1: - subsystems[subsystemIndex].setI(.01 * value); + subsystems[subsystemIndex].setI(subsystems[subsystemIndex].getI() + .01 * value); + break; case 2: - subsystems[subsystemIndex].setD(value); + subsystems[subsystemIndex].setD(subsystems[subsystemIndex].getD() + value); + break; } + log(); } @@ -64,10 +89,14 @@ public Command increaseCurrentPIDCommand(double value) { public Command decreaseCurrentPIDCommand(double value) { return Commands.runOnce(() -> this.increasePID(-value)); } + public Command PIDIncrementCommand() { return Commands.runOnce(() -> PIDIncrement()); } public Command PIDDecreaseCommand() { - return Commands.runOnce(() -> PIDIncrement()); + return Commands.runOnce(() -> PIDDecrease()); + } + public Command logCommand() { + return Commands.runOnce(() -> log()); } } From 84b0aefdfc2185fd30d3cddb67e47c0dddac5c2f Mon Sep 17 00:00:00 2001 From: Jacob Hotz <77470805+Jacob1010-h@users.noreply.github.com> Date: Sat, 10 Feb 2024 21:56:59 -0800 Subject: [PATCH 5/5] fix title case --- src/main/java/frc/robot/util/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 0cd9b35c..382a75ca 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -99,7 +99,7 @@ public static final class DriveConstants { } - public static final class tuningConstants implements Logged { + 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;