From c14f8125caa6c9b02b93caec2738f130e57ea1d7 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:08:56 -0600 Subject: [PATCH] alignToShelf READY TO TEST --- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 50fcd85..54a7afd 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -99,10 +99,11 @@ public class RobotContainer { () -> m_robotLimeLight.getNumTapes() <= 2 ); + // TODO: find actual distance private SequentialCommandGroup alignToShelf = new SequentialCommandGroup( new RotateToAngle(m_robotSwerveDrive, 0.0), new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw()), - new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril()) // TODO: find distance + new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril()) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); public SequentialCommandGroup place = null;