From d22804b7f232a1a774e2203f6c4008b60c6ff176 Mon Sep 17 00:00:00 2001
From: cuttestkittensrule <mangoiscute95@gmail.com>
Date: Sat, 9 Nov 2024 09:00:01 -0800
Subject: [PATCH] Added reset positioning button

---
 .../src/main/java/com/team2813/Constants.java         |  1 +
 .../src/main/java/com/team2813/RobotContainer.java    | 11 +++++++++--
 .../src/main/java/com/team2813/subsystems/Drive.java  |  5 +++++
 3 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/SharpShooter/src/main/java/com/team2813/Constants.java b/SharpShooter/src/main/java/com/team2813/Constants.java
index 586cc5b..9e3a1ae 100644
--- 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
index 8f6d8b6..85fdf6f 100644
--- 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
index 07f52a2..3fcb1b2 100644
--- 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());