diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 4036fec2..65d39bd8 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -234,6 +234,7 @@ public static class Tramp { } public static class Targeting { + // TODO: tune these public static final double ROT_KP = 1.18; public static final double ROT_KI = 0.0; public static final double ROT_KD = 0.0; diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java index 6bfd550d..0bf10fe7 100644 --- a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -27,6 +27,9 @@ public class DriveWithSpeakerTargetingCommand extends Command { private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); + private boolean isFlipped; + private Pose2d speakerPose; + public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) { this.drivetrain = drivetrain; this.controller = controller; @@ -37,6 +40,11 @@ public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxContro @Override public void initialize() { rotController.reset(); + + isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; } @Override @@ -45,10 +53,6 @@ public void execute() { rotController.setPID(kP.get(), kI.get(), kD.get()); } - boolean isFlipped = DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - Pose2d speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; - Rotation2d targetRot = drivetrain.getPose().minus(speakerPose).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180)); @@ -59,7 +63,6 @@ public void execute() { double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); - } @Override diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java index e040f576..60f2b29e 100644 --- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java @@ -15,6 +15,8 @@ public class SwerveDriveCommand extends Command { private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); + private boolean isFlipped; + public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) { this.drivetrain = drivetrain; this.controller = controller; @@ -22,11 +24,14 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle } @Override - public void execute() { - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + public void initialize() { + isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } + @Override + public void execute() { double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1);