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 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 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 xJoystickSupplier,