Add standalone versions of controlled rotate

frc868 · Mar 9, 2024 · e7cecc1 · e7cecc1
1 parent 3b8f2bc
commit e7cecc1
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 13 deletions.
diff --git a/houndutil b/houndutil
diff --git 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
@@ -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,