Added reset positioning button

Prospect-Robotics · Nov 9, 2024 · d22804b · d22804b
1 parent fd35af3
commit d22804b
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 2 deletions.
diff --git a/SharpShooter/src/main/java/com/team2813/Constants.java b/SharpShooter/src/main/java/com/team2813/Constants.java
@@ -38,6 +38,7 @@ public static class DriverConstants {
     public static Trigger MANUAL_OUTTAKE = DRIVER_CONTROLLER.R2();
     public static Trigger MANUAL_INTAKE = DRIVER_CONTROLLER.R1();
     public static Trigger TEST = DRIVER_CONTROLLER.cross();
+    public static Trigger RESET_POSITIONING = DRIVER_CONTROLLER.povUp();
 
     private DriverConstants() {
       throw new AssertionError("Not instantiable");

diff --git a/SharpShooter/src/main/java/com/team2813/RobotContainer.java b/SharpShooter/src/main/java/com/team2813/RobotContainer.java
@@ -38,11 +38,11 @@ public RobotContainer() {
                         drive,
                         () ->
                                 Units.MetersPerSecond.of(
-                                        modifyAxis(DRIVER_CONTROLLER.getLeftX())
+                                        -modifyAxis(DRIVER_CONTROLLER.getLeftY())
                                                 * Drive.MAX_VELOCITY),
                         () ->
                                 Units.MetersPerSecond.of(
-                                        -modifyAxis(DRIVER_CONTROLLER.getLeftY())
+                                        -modifyAxis(DRIVER_CONTROLLER.getLeftX())
                                                 * Drive.MAX_VELOCITY),
                         () ->
                                 Units.RadiansPerSecond.of(
@@ -199,6 +199,13 @@ private void configureBindings() {
 
         SLOW_MODE.onTrue(new InstantCommand(() -> drive.enableSlowMode(true), drive));
         SLOW_MODE.onFalse(new InstantCommand(() -> drive.enableSlowMode(false), drive));
+
+        RESET_POSITIONING.whileTrue(
+                new SequentialCommandGroup(
+                        new WaitCommand(1),
+                        new InstantCommand(drive::resetRotation, drive)
+                )
+        );
     }
 
     public Command getAutonomousCommand() {

diff --git a/SharpShooter/src/main/java/com/team2813/subsystems/Drive.java b/SharpShooter/src/main/java/com/team2813/subsystems/Drive.java
@@ -148,6 +148,7 @@ public Drive() {
       int temp = i;
       Shuffleboard.getTab("swerve").addDouble(String.format("Module [%d] position", i), () -> getPosition(temp));
     }
+    drivetrain.seedFieldRelative();
   }
 
   private double getPosition(int moduleId) {
@@ -164,6 +165,10 @@ public Pose2d getAutoPose() {
   public void resetOdometry(Pose2d currentPose) {
     drivetrain.seedFieldRelative(currentPose);
   }
+
+  public void resetRotation() {
+    drivetrain.seedFieldRelative();
+  }
 
   public void stop() {
     drivetrain.setControl(new SwerveRequest.Idle());