Add relocalize button to CD controller

FRCTeam1987 · May 31, 2024 · 671f901 · 671f901
1 parent 4873f1d
commit 671f901
Showing 1 changed file with 15 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -360,6 +361,20 @@ private void configureCoDriverController() {
             new ParallelDeadlineGroup(
                 new IntakeNoteSequence(SHOOTER, INTAKE, WRIST, ELEVATOR),
                 new DriveToNoteAuto(DRIVETRAIN, INTAKE_PHOTON, SHOOTER, INTAKE, WRIST, ELEVATOR)));
+
+    CO_DRIVER_CONTROLLER
+        .leftStick()
+        .onTrue(
+            new InstantCommand(
+                () -> {
+                  if (CommandSwerveDrivetrain.getAlliance() == Alliance.Blue) {
+                    DRIVETRAIN.seedFieldRelative(
+                        new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0)));
+                  } else {
+                    DRIVETRAIN.seedFieldRelative(
+                        new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180)));
+                  }
+                }));
   }
 
   private void configureDrivetrain() {