diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto new file mode 100644 index 0000000..e51e2f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 4 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index a04fbe0..b9b9e6b 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -28,12 +28,6 @@ "pathName": "Reef 3 to Bottom Feed" } }, - { - "type": "named", - "data": { - "name": "align-feed" - } - }, { "type": "named", "data": { @@ -58,12 +52,6 @@ "pathName": "Reef 2 to Bottom Feed" } }, - { - "type": "named", - "data": { - "name": "align-feed" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index bef1742..6c86f54 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.0603125, - "y": 1.0390625 + "x": 1.0807449494949497, + "y": 1.0875631313131315 }, "prevControl": null, "nextControl": { - "x": 2.3463856916648673, - "y": 1.797152889765366 + "x": 2.3668181411598166, + "y": 1.8456535210784974 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index e904b37..6a79779 100644 --- a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.595959595959596, - "y": 3.0289141414141416 + "x": 7.006439393939393, + "y": 3.1508838383838382 }, "prevControl": null, "nextControl": { - "x": 6.2521875, - "y": 2.6234375000000005 + "x": 6.498232323232323, + "y": 2.480050505050505 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6934974747474745 }, "prevControl": { - "x": 5.630625, - "y": 2.148125 + "x": 5.979861111111112, + "y": 1.992171717171717 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -178.9832812445108 + "rotation": 124.11447294534119 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 44a5838..4d15bb2 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.129687499999999, - "y": 1.9409375 + "x": 7.016603535353535, + "y": 1.9718434343434341 }, "prevControl": null, "nextControl": { - "x": 6.072698863636364, - "y": 1.0434943181818186 + "x": 6.000189393939394, + "y": 1.6364267676767676 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2771875, - "y": 2.61125 + "x": 5.47165404040404, + "y": 2.1141414141414145 }, "prevControl": { - "x": 5.504318181818181, - "y": 2.1902272727272725 + "x": 5.86940340909091, + "y": 1.7593308080808083 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 118.99790479482637 + "rotation": 124.8244891569568 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 179.5295681977034 + "rotation": 126.34745820888533 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index 42a1ac9..f42fbe9 100644 --- a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.565467171717171, - "y": 0.8334595959595947 + "x": 7.057260101010102, + "y": 0.8537878787878788 }, "prevControl": null, "nextControl": { - "x": 6.625889381464077, - "y": 1.4477106264692527 + "x": 6.315277777777778, + "y": 1.5144570707070704 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6234375 }, "prevControl": { - "x": 5.721332938841585, - "y": 2.265481302925542 + "x": 5.624116161616161, + "y": 2.205618686868687 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": -179.94258861633824 + "rotation": 132.5633517531898 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index 562a21c..bd485e9 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.680625, - "y": 2.61125 + "x": 3.749318181818182, + "y": 2.868295454545454 }, "prevControl": null, "nextControl": { - "x": 3.2541591082344743, - "y": 2.3264921263556886 + "x": 3.093244949494949, + "y": 2.2869318181818175 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.12125, - "y": 1.0025 + "x": 1.3246843434343436, + "y": 1.5347853535353522 }, "prevControl": { - "x": 1.3989083079519464, - "y": 1.1682576997602765 + "x": 1.802550505050505, + "y": 2.1992045454545446 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 58.10920819815426 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 48b8fd6..9df45e7 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.265, - "y": 2.61125 + "x": 5.268371212121212, + "y": 2.4597222222222217 }, "prevControl": null, "nextControl": { - "x": 4.137208815063533, - "y": 2.0700494801809173 + "x": 4.670075757575758, + "y": 1.2531597222222217 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.096875, - "y": 1.0390625 + "x": 1.1010732323232322, + "y": 1.00625 }, "prevControl": { - "x": 1.3299317623663032, - "y": 1.1501468126694903 + "x": 1.786268939393939, + "y": 2.2439551767676766 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 121.42956561483854 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index cbd1bbf..42d3c4c 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -20,7 +20,7 @@ "y": 1.0146875 }, "prevControl": { - "x": 1.95, + "x": 1.9500000000000002, "y": 2.1115625000000002 }, "nextControl": null, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index adb29c4..a37693b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -193,7 +193,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0.2, 0.1); // TODO: TEST } public static final class Configurations { @@ -294,7 +294,7 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(5,0.4,0.0); + public static final Gains XY_GAINS = new Gains(5,0.6,0.0); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); @@ -316,10 +316,10 @@ public final class Constants { public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); - public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); - public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c938162..5eed240 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -317,12 +317,19 @@ public class RobotContainer { private Boolean operatorManualMode = false; - public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral()); - public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral()); + // public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral()); + + // public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); + // public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral())); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -334,15 +341,15 @@ public class RobotContainer { NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), - new Translation2d(), 400, true + new Translation2d(), 300, true ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); - NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left); - NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right); - NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left); - NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right); - NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left); - NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); + NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); + NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); + NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { m_robotElevator.transitionState(CoordinationState.PrimedFour); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 7e9d1db..e0320ad 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -95,12 +95,13 @@ public class GotoLastApril extends Command { roterr += 360; } - SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); - SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); - SmartDashboard.putNumber("Rotational PID error", roterr); + // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID error", roterr); - // SmartDashboard.putNumber("PID X Error", xerr); - // SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID Rot Error", roterr); xoutput = xPID.update(xerr); youtput = yPID.update(yerr); @@ -128,7 +129,7 @@ public class GotoLastApril extends Command { .rotateBy(targetpos.getRotation()) .getCos()); - swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation()); + swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); } diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index d4c0c82..050cff2 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -41,7 +41,7 @@ public class LidarAlign extends Command { this.currentFinderTick = 0; this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; - this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.headedRight = (GotoLastApril.tagRelativeXError < 0); this.additionalDistance = 0; this.bounces = 0; } diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java index 31b189e..8db5f64 100644 --- a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java +++ b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java @@ -34,24 +34,26 @@ public class WhileTrueCommand extends Command { */ @SuppressWarnings("this-escape") public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { - m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand"); + m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); - CommandScheduler.getInstance().registerComposedCommands(whileTrue); + //CommandScheduler.getInstance().registerComposedCommands(whileTrue); - addRequirements(whileTrue.getRequirements()); + // addRequirements(whileTrue.getRequirements()); } @Override public void initialize() { if(m_condition.getAsBoolean()) - m_whileTrue.initialize(); + m_whileTrue.initialize(); } @Override public void execute() { m_whileTrue.execute(); + System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); + if(!m_whileTrue.isFinished()) return; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6f7ef5e..1f7f9a1 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -141,7 +141,7 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); stopped = false; if (fieldRelative) { @@ -213,13 +213,28 @@ public class SwerveDrive extends Subsystem { .withTargetDirection(rightStick.getAngle())); } + public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + ); + swerveDriveTrain.setControl(ctrl); + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + .withTargetDirection(heading); ctrl.HeadingController.setPID( SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,