diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3fcac20..7635cf3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,9 @@ import frc4388.robot.commands.auto.Bounce; import frc4388.robot.commands.auto.DriveOffLineBackward; import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; +import frc4388.robot.commands.auto.EightBallMid; import frc4388.robot.commands.auto.FiveBallAutoMiddle; +import frc4388.robot.commands.auto.FiveBallBottom; import frc4388.robot.commands.auto.GalacticSearch; import frc4388.robot.commands.auto.IdentifyPath; import frc4388.robot.commands.auto.SequentialTest; diff --git a/src/main/java/frc4388/robot/commands/auto/EightBallMid.java b/src/main/java/frc4388/robot/commands/auto/EightBallMid.java index 68d89f3..c34ed81 100644 --- a/src/main/java/frc4388/robot/commands/auto/EightBallMid.java +++ b/src/main/java/frc4388/robot/commands/auto/EightBallMid.java @@ -35,20 +35,29 @@ public class EightBallMid extends SequentialCommandGroup { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); addCommands( - //TODO REWRITE //Shoot and Extend Intake - new CalibrateShooter(shooter, shooterAim, shooterHood), - new ParallelDeadlineGroup( - new Wait(drive,5), - new TrackTarget(shooterAim), - new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle())), + new ParallelCommandGroup( + new CalibrateShooter(shooter, shooterAim, shooterHood), new RunExtenderOutIn(intake) ), - //Intake and Path + //Intake and Path0 then shoot new ParallelDeadlineGroup( paths[0], new RunIntake(intake) ), + new ParallelDeadlineGroup( + new Wait(drive,5), + new TrackTarget(shooterAim), + new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle())) + ), + + //Path1 and Intake, then path two + new ParallelDeadlineGroup( + paths[1], + new RunIntake(intake) + ), + paths[2], + //Shoot new ParallelDeadlineGroup( new Wait(drive,5), diff --git a/src/main/java/frc4388/robot/commands/auto/FiveBallBottom.java b/src/main/java/frc4388/robot/commands/auto/FiveBallBottom.java index 27ba3b8..4cfabc3 100644 --- a/src/main/java/frc4388/robot/commands/auto/FiveBallBottom.java +++ b/src/main/java/frc4388/robot/commands/auto/FiveBallBottom.java @@ -34,19 +34,19 @@ public class FiveBallBottom extends SequentialCommandGroup { public FiveBallBottom(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); - //TODO Rewrite addCommands( - //Shoot and Extend Intake - new CalibrateShooter(shooter, shooterAim, shooterHood), - new ParallelDeadlineGroup( - new Wait(drive,5), - new TrackTarget(shooterAim), - new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle())), + //Extend Intake and calibrate + new ParallelCommandGroup( + new CalibrateShooter(shooter, shooterAim, shooterHood), new RunExtenderOutIn(intake) ), - //Intake and Path + //Intake and Paths new ParallelDeadlineGroup( - paths[0], + new SequentialCommandGroup( + paths[0], + paths[1], + paths[2] + ), new RunIntake(intake) ), //Shoot