return to normal

Iron-Panthers · Oct 27, 2023 · 384896b · 384896b
1 parent 5e9969f
commit 384896b
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/commands/RotateVectorDriveCommand.java b/src/main/java/frc/robot/commands/RotateVectorDriveCommand.java
@@ -71,11 +71,11 @@ public void execute() {
     double rotY = rotationYSupplier.getAsDouble();
     boolean isRobotRelative = isRobotRelativeSupplier.getAsBoolean();
 
-    double targetAngle = Util.vectorToAngle(-rotX, -rotY);
+    double targetAngle = Util.angleSnap(Util.vectorToAngle(-rotX, -rotY), angles);
 
     // if stick magnitude is greater then rotate angle mag
     if (Util.vectorMagnitude(rotX, rotY) > Drive.ROTATE_VECTOR_MAGNITUDE) {
-      angle = targetAngle;
+      angle = isRobotRelative ? Util.normalizeDegrees(targetAngle + initialAngle) : targetAngle;
     }
 
     drivebaseSubsystem.driveAngle(new Pair<>(x, y), angle);