From 893deb1917c941dab2001ca7b8d0270a068e3326 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Sat, 9 Nov 2024 22:54:17 -0800
Subject: [PATCH] Angle snapping should work (me when I learn how the code I
 wrote works)

---
 src/main/java/frc/robot/RobotContainer.java          | 8 ++++----
 src/main/java/frc/robot/subsystems/swerve/Drive.java | 7 ++-----
 2 files changed, 6 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 592df70..6faac9a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -473,7 +473,7 @@ private void configureBindings() {
                         driverA.getLeftY(),
                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -39 : 39),
                 interrupted -> {},
-                () -> Math.abs(swerve.getAngularError(0)) < 5));
+                () -> true));
     // amp
     driverA
         .povLeft()
@@ -486,7 +486,7 @@ private void configureBindings() {
                         driverA.getLeftY(),
                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -90 : 90),
                 interrupted -> {},
-                () -> Math.abs(swerve.getAngularError(0)) < 5));
+                () -> true));
     // shuttle
     driverA
         .povRight()
@@ -501,7 +501,7 @@ private void configureBindings() {
                   superstructure.setTargetState(SuperstructureState.SHUTTLE);
                 },
                 interrupted -> {},
-                () -> Math.abs(swerve.getAngularError(0)) < 5));
+                () -> true));
     // speaker
     driverA
         .povDown()
@@ -510,7 +510,7 @@ private void configureBindings() {
                 () -> {},
                 () -> swerve.driveAnglePeriodic(driverA.getLeftX(), driverA.getLeftY(), 0),
                 interrupted -> {},
-                () -> Math.abs(swerve.getAngularError(0)) < 5));
+                () -> true));
   }
 
   private void configureAutos() {}
diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java
index 504dea5..d95dcc1 100644
--- a/src/main/java/frc/robot/subsystems/swerve/Drive.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java
@@ -97,10 +97,7 @@ public void periodic() {
   }
 
   public void driveTeleopController(double xAxis, double yAxis) {
-    if (driveMode != DriveModes.TELEOP) {
-      driveMode = DriveModes.TELEOP;
-      targetAngle = arbitraryYaw.getDegrees();
-    }
+    
     driveAnglePeriodic(xAxis, yAxis, targetAngle);
   }
 
@@ -118,7 +115,7 @@ public void zero() {
   public void driveAnglePeriodic(double xAxis, double yAxis, double targetAngle) {
     this.targetAngle = targetAngle;
     double angularDifference = getAngularError(targetAngle);
-
+    
     double rotationValue = rotController.calculate(angularDifference);
 
     // we are treating this like a joystick, so -1 and 1 are its lower and upper bound