diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 882485e8..527ce475 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -43,7 +44,7 @@ public static class ElevatorConstants { public static final double EXTENSION_D = 0; public static final double EXTENSION_TOLERANCE = 0.008; - public static final double POSITION_CONVERSION_FACTOR = 1 /26.357; // Units.inchesToMeters(30.)/63.5; + public static final double POSITION_CONVERSION_FACTOR = 1 / 26.357; // Units.inchesToMeters(30.)/63.5; public static final double VELOCITY_CONVERSION_FACTOR = 1; public static final double DOWN_POWER = -0.001; //the motor power to make the elevator move down slowly @@ -184,6 +185,44 @@ public static class AutoAlignConstants { public static final Pose2d RED_AMP_POSE = new Pose2d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.00 - robotRadius), Rotation2d.fromDegrees(90)); + /* Stage poses such that the climb hooks are directly above the chains. Poses are defined in terms of the + * corresponding apriltag's pose transformed by a constant distance to the chain. "Stage Left", in this case, + * refers to the left third of the stage when facing the stage from the driver station. We are aware that this + * naming scheme differs from the theatre convention. */ + private static final Transform2d STAGE_TAG_TO_ROBOT = new Transform2d(Units.inchesToMeters(17.0), + Units.inchesToMeters(0), + Rotation2d.fromDegrees(180)); + + public static final Pose2d BLUE_STAGE_BACK_POSE = new Pose2d(Units.inchesToMeters(209.48), + Units.inchesToMeters(161.62), + Rotation2d.fromDegrees(0)) + .transformBy(STAGE_TAG_TO_ROBOT); + + public static final Pose2d BLUE_STAGE_LEFT_POSE = new Pose2d(Units.inchesToMeters(182.73), + Units.inchesToMeters(177.10), + Rotation2d.fromDegrees(120)) + .transformBy(STAGE_TAG_TO_ROBOT); + + public static final Pose2d BLUE_STAGE_RIGHT_POSE = new Pose2d(Units.inchesToMeters(182.73), + Units.inchesToMeters(146.19), + Rotation2d.fromDegrees(240)) + .transformBy(STAGE_TAG_TO_ROBOT); + + public static final Pose2d RED_STAGE_BACK_POSE = new Pose2d(Units.inchesToMeters(441.74), + Units.inchesToMeters(161.62), + Rotation2d.fromDegrees(180)) + .transformBy(STAGE_TAG_TO_ROBOT); + + public static final Pose2d RED_STAGE_LEFT_POSE = new Pose2d(Units.inchesToMeters(468.69), + Units.inchesToMeters(146.19), + Rotation2d.fromDegrees(300)) + .transformBy(STAGE_TAG_TO_ROBOT); + + public static final Pose2d RED_STAGE_RIGHT_POSE = new Pose2d(Units.inchesToMeters(468.69), + Units.inchesToMeters(177.10), + Rotation2d.fromDegrees(60)) + .transformBy(STAGE_TAG_TO_ROBOT); + } /** Constants for auton. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1eab8647..a817425d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,6 @@ import static frc.robot.Constants.VisionConstants.NOTE_CAMERA; import java.util.EnumSet; -import java.util.function.BooleanSupplier; import com.choreo.lib.ChoreoTrajectory; import com.fasterxml.jackson.databind.util.Named; @@ -224,11 +223,11 @@ public RobotContainer() { } try { - driverCamera = new UsbCamera("fisheye", 0); - driverCamera.setVideoMode(PixelFormat.kMJPEG, 160, 120, 30); - driverCamera.setExposureManual(40); - driverCameraServer = new MjpegServer("m1", 1181); - driverCameraServer.setSource(driverCamera); + driverCamera = new UsbCamera("fisheye", 0); + driverCamera.setVideoMode(PixelFormat.kMJPEG, 160, 120, 30); + driverCamera.setExposureManual(40); + driverCameraServer = new MjpegServer("m1", 1181); + driverCameraServer.setSource(driverCamera); } catch (Exception e) { System.out.print(e); } @@ -349,14 +348,14 @@ private void configureBindings() { /* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the * amp and deposit its note. Releasing the button will stop the robot (and the path finding). */ // TODO: Bring back LED integration. - driveController.getAutoAmp().onTrue( - new AutoAmpSequence(fmsSubsystem, - swerveSubsystem, - elevatorSubsystem, - intakePivotSubsystem, - intakeRollerSubsystem) - - .onlyWhile(driveController.getAutoAmp()) + driveController.getAutoAmp().whileTrue( + Commands.deferredProxy(() -> new AutoAmpSequence( + fmsSubsystem, + swerveSubsystem, + elevatorSubsystem, + intakePivotSubsystem, + intakeRollerSubsystem + )) ); // DEPRECATED AMP ALIGN @@ -366,9 +365,22 @@ private void configureBindings() { // AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()), // new ConditionalWaitCommand( // () -> !driveController.getAmpAlign().getAsBoolean())) - // ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1))) + // ).andThen( + // new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)) + // ) // ); + /* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its + * climb hooks will end up directly above the center of the nearest chain. */ + driveController.getStageAlignButton().whileTrue( + Commands.deferredProxy(() -> AlignCommand.getStageAlignCommand( + swerveSubsystem, + swerveSubsystem::getRobotPosition, + fmsSubsystem::isRedAlliance + )) + ); + + /* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */ driveController.getNoteAlign().onTrue( new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem, diff --git a/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java b/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java index da7bf6c6..08c2bb17 100644 --- a/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java +++ b/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java @@ -30,7 +30,7 @@ public AutoAmpSequence(FieldManagementSubsystem fms, /* Prepares the mechanisms for amping while path-finding the robot to its amping position. */ Commands.parallel( new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem), - AlignCommand.getAmpAlignCommand(swerveSubsystem, fms.isRedAlliance()) + AlignCommand.getAmpAlignCommand(swerveSubsystem, fms::isRedAlliance) ), /* Deposits the note into the amp. */ diff --git a/src/main/java/frc/robot/commands/swerve/AlignCommand.java b/src/main/java/frc/robot/commands/swerve/AlignCommand.java index 69e12133..cb9f9e84 100644 --- a/src/main/java/frc/robot/commands/swerve/AlignCommand.java +++ b/src/main/java/frc/robot/commands/swerve/AlignCommand.java @@ -1,5 +1,12 @@ package frc.robot.commands.swerve; +import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_BACK_POSE; +import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_LEFT_POSE; +import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_RIGHT_POSE; +import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_BACK_POSE; +import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_LEFT_POSE; +import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_RIGHT_POSE; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.geometry.Pose2d; @@ -7,6 +14,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.AutoAlignConstants; import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.util.Pose2dSupplier; +import java.util.function.BooleanSupplier; /** Contains functions to create auto-alignment commands. */ public class AlignCommand { @@ -38,31 +47,49 @@ public static Command getAlignCommand(Pose2d targetPose, SwerveSubsystem swerveS * Creates a PathPlanner align command using PathPlanner's pathfindToPose() and autoBuilder. * * @param swerveSubsystem The robot swerve subsystem to control - * @param isRed Whether or not we have a red alliance + * @param isRedSup True if the robot is on the red alliance, false if on blue. * @return Pathfinding Command that pathfinds and aligns the robot */ - public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, Boolean isRed) { + public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, BooleanSupplier isRedSup) { Pose2d targetPose; - if (isRed) { + if (isRedSup.getAsBoolean()) { targetPose = AutoAlignConstants.RED_AMP_POSE; } else { targetPose = AutoAlignConstants.BLUE_AMP_POSE; } + + return getAlignCommand(targetPose, swerveSubsystem); + } - Command command = AutoBuilder.pathfindToPose( - targetPose, - new PathConstraints( - 2.0, 2.0, - Units.degreesToRadians(720), Units.degreesToRadians(1080) - ), - 0, - 0.0 - ); + /** + * Generates a holonomic path from the robot to the nearest stage face. The robot should end up with its climb hooks + * aligned directly above the chain. + * + * @param swerveSubsystem The swerve drive to move the robot to its target. + * @param currentPoseSup A function that returns the current pose of the robot. + * @param isRedSup True if the robot is on the red alliance, false if on the blue alliance. + * @return Pathfinding Command to bring the robot to its target position. + */ + public static Command getStageAlignCommand(SwerveSubsystem swerveSubsystem, + Pose2dSupplier currentPoseSup, BooleanSupplier isRedSup) { + Pose2d currentPose = currentPoseSup.getPose2d(); + Pose2d targetPose = new Pose2d(); + double distance = Double.MAX_VALUE; - command.addRequirements(swerveSubsystem); - - return command; + Pose2d[] possibleTargets = isRedSup.getAsBoolean() + ? new Pose2d[]{RED_STAGE_BACK_POSE, RED_STAGE_LEFT_POSE, RED_STAGE_RIGHT_POSE} + : new Pose2d[]{BLUE_STAGE_BACK_POSE, BLUE_STAGE_LEFT_POSE, BLUE_STAGE_RIGHT_POSE}; + + for (Pose2d possibleTarget : possibleTargets) { + double possibleDistance = currentPose.getTranslation().getDistance(possibleTarget.getTranslation()); + if (possibleDistance < distance) { + distance = possibleDistance; + targetPose = possibleTarget; + } + } + + return getAlignCommand(targetPose, swerveSubsystem); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java index b3b4c3d6..18a56c94 100644 --- a/src/main/java/frc/robot/controllers/BaseDriveController.java +++ b/src/main/java/frc/robot/controllers/BaseDriveController.java @@ -88,4 +88,7 @@ public abstract class BaseDriveController { * Gets the button to aim the shooter at the speaker. */ public abstract JoystickButton getShooterAimButton(); + + /** Returns the button to auto-align to the nearest stage face. */ + public abstract JoystickButton getStageAlignButton(); } diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java index 2fc362da..13b20c2e 100644 --- a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java +++ b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java @@ -15,6 +15,7 @@ public class DualJoystickDriveController extends BaseDriveController { private final JoystickButton leftMiddleButton = new JoystickButton(leftJoystick, 2); private final JoystickButton leftTopLeftButton = new JoystickButton(leftJoystick, 3); private final JoystickButton leftTopRightButton = new JoystickButton(leftJoystick, 4); + private final JoystickButton leftBottomLeftButton = new JoystickButton(leftJoystick, 5); private final Joystick rightJoystick = new Joystick(1); private final JoystickButton rightTrigger = new JoystickButton(rightJoystick, 1); @@ -100,5 +101,10 @@ public Boolean getSwerveAimMode() { @Override public JoystickButton getShooterAimButton() { return leftTopRightButton; - } + } + + @Override + public JoystickButton getStageAlignButton() { + return leftBottomLeftButton; + } } diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java index f10d2179..a72a9f67 100644 --- a/src/main/java/frc/robot/controllers/XboxDriveController.java +++ b/src/main/java/frc/robot/controllers/XboxDriveController.java @@ -21,6 +21,9 @@ public class XboxDriveController extends BaseDriveController { private final JoystickButton driveLStickButton = new JoystickButton( driveController, XboxController.Button.kLeftStick.value ); + private final JoystickButton driveRStickButton = new JoystickButton( + driveController, XboxController.Button.kRightStick.value + ); @Override public double getForwardPower() { @@ -77,7 +80,13 @@ public Boolean getSwerveAimMode() { return driveLStickButton.getAsBoolean(); } - public JoystickButton getShooterAimButton(){ + @Override + public JoystickButton getShooterAimButton() { return driveLStickButton; } + + @Override + public JoystickButton getStageAlignButton() { + return driveRStickButton; + } }