From c18f93be82b2107c6a7d01ca2173c023cbba6f73 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Mar 2023 14:48:09 -0600 Subject: [PATCH] dpad queueing WORKS!!! --- src/main/java/frc4388/robot/RobotContainer.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8da0eab..dba8ee9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -98,12 +98,21 @@ public class RobotContainer { // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); + // private SequentialCommandGroup alignToShelf = + // new SequentialCommandGroup( + // new RotateToAngle(m_robotSwerveDrive, 0.0), + // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), + // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), + // new RotateToAngle(m_robotSwerveDrive, 0.0), + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); + private SequentialCommandGroup alignToShelf = new SequentialCommandGroup( - new RotateToAngle(m_robotSwerveDrive, 0.0), + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), - new RotateToAngle(m_robotSwerveDrive, 0.0), + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));