From 06193efd831142448e256a4d0cc64f7a6f8fb27d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 7 Apr 2022 15:37:55 -0600 Subject: [PATCH] timeout for 2ball auto (untested + doubtful) --- src/main/java/frc4388/robot/RobotContainer.java | 16 +++++++++++++--- .../commands/ShooterCommands/TrackTarget.java | 5 ++++- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2668786..bc32f7b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -458,7 +458,17 @@ public class RobotContainer { ); } - SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), + private SequentialCommandGroup shoot(double storageRunTime, double timeout) { + return new SequentialCommandGroup( + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withTimeout(timeout), + new ParallelCommandGroup( + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).until(TrackTarget::isTargetNotLocked), + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true) + ) + ); + } + + SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), new ParallelCommandGroup( new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true), new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true) @@ -540,10 +550,10 @@ public class RobotContainer { // ! TWO BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE) SequentialCommandGroup twoBallAuto = new SequentialCommandGroup( idleDrumUntilShootingFirstBall(), - shoot(1.0), // TODO: optimize time + shoot(1.0, 4.0), // TODO: optimize time brakeStorage(0.1), intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget - shoot(2.3), // TODO: optimize time + shoot(5.0), // TODO: optimize time brakeStorage(0.1) ); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index bc2d8cd..9151974 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -48,7 +48,10 @@ public class TrackTarget extends CommandBase { ArrayList points = new ArrayList<>(); - private boolean targetLocked = false; + private static boolean targetLocked = false; + public static boolean isTargetNotLocked() { + return !targetLocked; + } private double velocityTolerance = 300.0; private double hoodTolerance = 5.0;