fix: flipping actually works in init you just have to go disconnected…

… -> disabled -> teleop
flamingchickens1540 · Feb 10, 2024 · 973ed09 · 973ed09
1 parent 46b3631
commit 973ed09
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 9 deletions.
diff --git 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
@@ -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
@@ -15,18 +15,23 @@ 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;
         addRequirements(drivetrain);
     }
 
     @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);