Angle snapping should work (me when I learn how the code I wrote works)

Iron-Panthers · Nov 10, 2024 · 893deb1 · 893deb1
1 parent 6964ffe
commit 893deb1
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 9 deletions.
diff --git 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
@@ -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