From d31c84e1f622a9e673a9cedd5c8133bc802baff2 Mon Sep 17 00:00:00 2001
From: Jay <jaykang445@gmail.com>
Date: Sat, 28 Oct 2023 16:38:15 -0700
Subject: [PATCH] (maybe) working field relative rotation?

---
 src/main/java/frc/robot/RobotContainer.java   | 29 ++++++++++++++-----
 .../commands/RotateVectorDriveCommand.java    | 22 +++++++-------
 2 files changed, 33 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cf7a249..942dc53 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -239,6 +239,19 @@ private void configureButtonBindings() {
                 rotationVelocity,
                 will.rightBumper()));
 
+    new Trigger(
+            () ->
+                Util.vectorMagnitude(will.getRightY(), will.getRightX())
+                    > Drive.ROTATE_VECTOR_MAGNITUDE)
+        .onTrue(
+            new RotateVectorDriveCommand(
+                drivebaseSubsystem,
+                translationXSupplier,
+                translationYSupplier,
+                will::getRightY,
+                will::getRightX,
+                will.rightBumper()));
+
 
     will.povUp()
         .onTrue(new RotateAngleDriveCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier, 0));
@@ -305,9 +318,9 @@ private void configureButtonBindings() {
     //         new ForceOuttakeSubsystemModeCommand(outtakeSubsystem,
     // OuttakeSubsystem.Modes.INTAKE));
 
-    will.rightTrigger()
-        .whileTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.SHELF_INTAKE))
-        .onFalse(new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED));
+    // will.rightTrigger()
+    //     .whileTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.SHELF_INTAKE))
+    //     .onFalse(new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED));
         
     
 
@@ -403,12 +416,12 @@ private void configureButtonBindings() {
               Constants.SCORE_STEP_MAP.get(scoreType),
               will.rightTrigger()));
 
-    will.leftTrigger()
-        .onTrue(
-            new HashMapCommand<>(
-                scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier()));
+    // will.leftTrigger()
+    //     .onTrue(
+    //         new HashMapCommand<>(
+    //             scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier()));
 
-    will.leftTrigger().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(1))));
+    // will.leftTrigger().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(1))));
     jason.b().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(-1))));
 
     // control the lights
diff --git a/src/main/java/frc/robot/commands/RotateVectorDriveCommand.java b/src/main/java/frc/robot/commands/RotateVectorDriveCommand.java
index 570204a..75076c3 100644
--- a/src/main/java/frc/robot/commands/RotateVectorDriveCommand.java
+++ b/src/main/java/frc/robot/commands/RotateVectorDriveCommand.java
@@ -71,11 +71,12 @@ public void execute() {
     double rotY = rotationYSupplier.getAsDouble();
     boolean isRobotRelative = isRobotRelativeSupplier.getAsBoolean();
 
-    double targetAngle = Util.angleSnap(Util.vectorToAngle(-rotX, -rotY), angles);
+    double targetAngle = Util.normalizeDegrees(Util.vectorToAngle(-rotX, -rotY));
+    //Util.vectorToAngle(-rotX, -rotY);
 
     // if stick magnitude is greater then rotate angle mag
     if (Util.vectorMagnitude(rotX, rotY) > Drive.ROTATE_VECTOR_MAGNITUDE) {
-      angle = isRobotRelative ? Util.normalizeDegrees(targetAngle + initialAngle) : targetAngle;
+      angle = targetAngle;
     }
 
     drivebaseSubsystem.driveAngle(new Pair<>(x, y), angle);
@@ -92,13 +93,14 @@ public void end(boolean interrupted) {
   @Override
   public boolean isFinished() {
     // are we at the angle we want
-    return Util.epsilonZero(
-            Util.relativeAngularDifference(drivebaseSubsystem.getDriverGyroscopeRotation(), angle),
-            Drive.ANGULAR_ERROR)
-        // is our rotational velocity low
-        && Util.epsilonEquals(drivebaseSubsystem.getRotVelocity(), 0, 10)
-        // are we not intentionally running pid to hold an angle
-        && Util.vectorMagnitude(rotationXSupplier.getAsDouble(), rotationYSupplier.getAsDouble())
-            <= Drive.ROTATE_VECTOR_MAGNITUDE;
+    // return Util.epsilonZero(
+    //         Util.relativeAngularDifference(drivebaseSubsystem.getDriverGyroscopeRotation(), angle),
+    //         Drive.ANGULAR_ERROR)
+    //     // is our rotational velocity low
+    //     && Util.epsilonEquals(drivebaseSubsystem.getRotVelocity(), 0, 10)
+    //     // are we not intentionally running pid to hold an angle
+    //     && Util.vectorMagnitude(rotationXSupplier.getAsDouble(), rotationYSupplier.getAsDouble())
+    //         <= Drive.ROTATE_VECTOR_MAGNITUDE;
+    return false;
   }
 }