From d0b5c15c2d0ac6dc625738d5f5e7690f11bf8d13 Mon Sep 17 00:00:00 2001
From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com>
Date: Wed, 6 Mar 2024 18:06:21 -0700
Subject: [PATCH] offsets

---
 src/main/java/frc4388/robot/Constants.java      | 8 ++++----
 src/main/java/frc4388/robot/RobotContainer.java | 8 +++++++-
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 6d949fa..3f606df 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -40,9 +40,9 @@ public final class Constants {
         public static final double TURBO_SPEED = 4.0;
     
         public static final class DefaultSwerveRotOffsets {
-          public static final double FRONT_LEFT_ROT_OFFSET = 130.957 + 90;
-          public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 45 + 90 ;//-202.588;
-          public static final double BACK_LEFT_ROT_OFFSET = 347.871 + 90;
+          public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 226.119140625;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
+          public static final double BACK_LEFT_ROT_OFFSET = -277.5587969;
           public static final double BACK_RIGHT_ROT_OFFSET = 52.646 + 90;
         }
 
@@ -67,7 +67,7 @@ public final class Constants {
         public static final class PIDConstants {
             public static final int SWERVE_SLOT_IDX = 0;
             public static final int SWERVE_PID_LOOP_IDX = 1;
-            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.0, 0, 1.0);
+            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 1.0);
         }
     
         public static final class AutoConstants {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 799debc..cf62103 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -8,6 +8,7 @@
 package frc4388.robot;
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -351,7 +352,12 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
         
-        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .whileTrue(new InstantCommand(() -> 
+            m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
+                                              new Translation2d(0, 0),
+                                true), m_robotSwerveDrive));
+
         
        //?  /* Operator Buttons */