From e7cecc10090a5d3cdcddd79e2a987b93449d8d71 Mon Sep 17 00:00:00 2001
From: superpenguin612 <74030080+superpenguin612@users.noreply.github.com>
Date: Fri, 8 Mar 2024 22:54:23 -0500
Subject: [PATCH] Add standalone versions of controlled rotate

---
 houndutil                                     |  2 +-
 src/main/java/frc/robot/Controls.java         | 12 ++----
 .../java/frc/robot/subsystems/Drivetrain.java | 40 +++++++++++++++++--
 3 files changed, 41 insertions(+), 13 deletions(-)

diff --git a/houndutil b/houndutil
index 7e3c7a4..1839cd7 160000
--- a/houndutil
+++ b/houndutil
@@ -1 +1 @@
-Subproject commit 7e3c7a418dd43c4d10cc03c97d6bcc1d604e9f66
+Subproject commit 1839cd7685fa63fa4728d2896e89c26c4b90d8cd
diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java
index 06ceba0..624f36c 100644
--- a/src/main/java/frc/robot/Controls.java
+++ b/src/main/java/frc/robot/Controls.java
@@ -34,17 +34,13 @@ public static void configureDriverControl(int port, Drivetrain drivetrain, Intak
         joystick.stickButton().onTrue(drivetrain.resetGyroCommand());
 
         joystick.centerBottomHatUp()
-                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(0),
-                        DriveMode.FIELD_ORIENTED));
+                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(0)));
         joystick.centerBottomHatLeft()
-                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(270),
-                        DriveMode.FIELD_ORIENTED));
+                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(270)));
         joystick.centerBottomHatDown()
-                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(180),
-                        DriveMode.FIELD_ORIENTED));
+                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(180)));
         joystick.centerBottomHatRight()
-                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(90),
-                        DriveMode.FIELD_ORIENTED));
+                .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(90)));
 
         joystick.triggerSoftPress().and(joystick.flipTriggerIn().negate())
                 .whileTrue(RobotCommands.targetSpeakerOnTheMoveCommand(drivetrain, shooter,
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 17af203..5d8a1db 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -734,20 +734,34 @@ public Command teleopDriveCommand(DoubleSupplier xSpeedSupplier, DoubleSupplier
     }
 
     @Override
-    public Command controlledRotateCommand(DoubleSupplier angle, DriveMode driveMode) {
+    public Command controlledRotateCommand(DoubleSupplier angle) {
         return Commands.run(() -> {
             if (!isControlledRotationEnabled) {
                 rotationController.reset(getRotation().getRadians());
             }
             isControlledRotationEnabled = true;
-            if (driveMode == DriveMode.FIELD_ORIENTED && DriverStation.getAlliance().isPresent()
-                    && DriverStation.getAlliance().get() == Alliance.Red)
+            if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
                 rotationController.setGoal(angle.getAsDouble() + Math.PI);
             else
                 rotationController.setGoal(angle.getAsDouble());
         }).withName("drivetrain.controlledRotate");
     }
 
+    public Command standaloneControlledRotateCommand(DoubleSupplier angle) {
+        return runOnce(() -> {
+            if (!isControlledRotationEnabled) {
+                rotationController.reset(getRotation().getRadians());
+            }
+            isControlledRotationEnabled = true;
+            if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
+                rotationController.setGoal(angle.getAsDouble() + Math.PI);
+            else
+                rotationController.setGoal(angle.getAsDouble());
+        }).andThen(run(() -> {
+            drive(new ChassisSpeeds(0, 0, rotationController.calculate(getRotation().getRadians())), driveMode);
+        })).withName("drivetrain.standaloneControlledRotate");
+    }
+
     @Override
     public Command disableControlledRotateCommand() {
         return Commands.runOnce(
@@ -942,7 +956,25 @@ public Command targetSpeakerCommand(Supplier<Pose3d> targetPose) {
             Rotation2d rot = new Rotation2d(diff.getX(), diff.getY());
             rot = rot.plus(new Rotation2d(Math.PI));
             return rot.getRadians();
-        }, DriveMode.FIELD_ORIENTED);
+        });
+    }
+
+    public Command standaloneTargetSpeakerCommand() {
+        return standaloneTargetSpeakerCommand(() -> FieldConstants.SPEAKER_TARGET);
+    }
+
+    public Command standaloneTargetSpeakerCommand(Supplier<Pose3d> targetPose) {
+        return standaloneControlledRotateCommand(() -> {
+            Pose2d target = DriverStation.getAlliance().isPresent()
+                    && DriverStation.getAlliance().get() == Alliance.Red
+                            ? Reflector.reflectPose2d(targetPose.get().toPose2d(),
+                                    FieldConstants.FIELD_LENGTH)
+                            : targetPose.get().toPose2d();
+            Transform2d diff = getPose().minus(target);
+            Rotation2d rot = new Rotation2d(diff.getX(), diff.getY());
+            rot = rot.plus(new Rotation2d(Math.PI));
+            return rot.getRadians();
+        });
     }
 
     public Command targetStageCommand(Supplier<Double> xJoystickSupplier,