From 7b7a0e235679e43cb6fdd57fdaab13d8da1f43b7 Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Thu, 7 Apr 2022 17:03:23 -0600 Subject: [PATCH] Add simplified 3 ball auto test --- .../java/frc4388/robot/RobotContainer.java | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e6593a0..f97adf9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -154,6 +154,7 @@ public class RobotContainer { autoChooser.addOption("OneBallAuto", oneBallAuto); autoChooser.setDefaultOption("TwoBallAuto", twoBallAuto); autoChooser.addOption("ThreeBallAuto", threeBallAuto); + autoChooser.addOption("testAuto", testAuto); SmartDashboard.putData("AutoChooser", autoChooser); @@ -573,6 +574,34 @@ public class RobotContainer { brakeStorage(0.1) ); + private final CommandGroupBase testAuto = CommandGroupBase.sequence( + // Preloaded Ball + new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstBallTarget"), + new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstBallFeed"), + new WaitCommand(3.0).withName("FirstBallShootTimer").alongWith(new RunExtender(m_robotExtender).withName("DeployExtender")).withName("ShootDeployExtender"), + new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstBallStopFeed"), + // Second Ball + new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"), + buildAuto(3.0, 3.0, "JMove1").withName("JMove1"), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("SecondBallTarget"), + new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("SecondBallFeed"), + new WaitCommand(3.0).withName("SecondBallShootTimer"), + new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("SecondBallStopFeed"), + // Third Ball + new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"), + buildAuto(3.0, 3.0, "JMove2").withName("JMove2"), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget"), + new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"), + new WaitCommand(3.0).withName("ThirdBallShootTimer"), + new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"), + new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"), + new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed") + ); + /** * Use this to pass the autonomous command to the main {@link Robot} class. *