added shoot and intake pose override controls

Talon540Programming · Mar 15, 2024 · 310720b · 310720b
1 parent 2e7ba69
commit 310720b
Show file tree

Hide file tree

Showing 7 changed files with 63 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -176,6 +176,19 @@ private void configureButtonBindings() {
             () -> PoseEstimator.getInstance().getPose(),
             m_drive::getVelocity,
             controlsInterface));
+
+    // Shoot and intake override
+    controlsInterface.shootPoseOverride().onTrue(
+        Commands.runOnce(
+            () -> {
+                PoseEstimator.getInstance().resetPose(Constants.Poses.SUBWF_SHOOT_POSE);
+            }));
+
+    controlsInterface.feederIntakePoseOverride().onTrue(
+        Commands.runOnce(
+            () -> {
+                PoseEstimator.getInstance().resetPose(Constants.Poses.FEEDER_INTAKE_POSE);
+            }));
   }
 
   public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -102,4 +103,9 @@ public static class Intake {
             Units.inchesToMeters(7.414499),
             new Rotation3d(0.0, 0.0, 0.0));
   }
+
+  public static class Poses {
+    public static final Pose2d SUBWF_SHOOT_POSE = new Pose2d(0, 0, null); //TODO
+    public static final Pose2d FEEDER_INTAKE_POSE = new Pose2d(0, 0, null); //TODO
+  }
 }
diff --git a/src/main/java/frc/robot/oi/DriveOI.java b/src/main/java/frc/robot/oi/DriveOI.java
@@ -16,4 +16,8 @@ public interface DriveOI {
   public Trigger shooterAngleLock();
 
   public Trigger trajectoryOverride();
+
+  public Trigger shootPoseOverride();
+
+  public Trigger feederIntakePoseOverride();
 }
diff --git a/src/main/java/frc/robot/oi/DualXbox.java b/src/main/java/frc/robot/oi/DualXbox.java
@@ -46,4 +46,14 @@ public Trigger trajectoryOverride() {
   public Trigger shoot() {
     return operatorController.x();
   }
+
+  @Override
+  public Trigger shootPoseOverride() {
+    return operatorController.a();
+  }
+
+  @Override
+  public Trigger feederIntakePoseOverride() {
+    return operatorController.x();
+  }
 }
diff --git a/src/main/java/frc/robot/oi/SinglePS4.java b/src/main/java/frc/robot/oi/SinglePS4.java
@@ -45,4 +45,14 @@ public Trigger trajectoryOverride() {
   public Trigger shoot() {
     return controller.touchpad();
   }
+
+  @Override
+  public Trigger shootPoseOverride() {
+    return controller.L2();
+  }
+
+  @Override
+  public Trigger feederIntakePoseOverride() {
+    return controller.L3();
+  }
 }
diff --git a/src/main/java/frc/robot/oi/SingleXbox.java b/src/main/java/frc/robot/oi/SingleXbox.java
@@ -45,4 +45,14 @@ public Trigger trajectoryOverride() {
   public Trigger shoot() {
     return controller.rightTrigger();
   }
+
+  @Override
+  public Trigger shootPoseOverride() {
+    return controller.a();
+  }
+
+  @Override
+  public Trigger feederIntakePoseOverride() {
+    return controller.x();
+  }
 }
diff --git a/src/main/java/frc/robot/oi/SrimanXbox.java b/src/main/java/frc/robot/oi/SrimanXbox.java
@@ -45,4 +45,14 @@ public Trigger trajectoryOverride() {
   public Trigger shoot() {
     return controller.rightTrigger();
   }
+
+  @Override
+  public Trigger shootPoseOverride() {
+    return controller.a();
+  }
+
+  @Override
+  public Trigger feederIntakePoseOverride() {
+    return controller.x();
+  }
 }