Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add simplified 3 ball auto test

Browse Source
This commit is contained in:
nathanrsxtn
2022-04-07 17:03:23 -06:00
parent 7f5d9318ec
commit 7b7a0e2356
1 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+29
View File
@@ -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.
*