diff --git a/src/main/java/frc/robot/commands/Autonomous/OneShotSequenceMiddle.java b/src/main/java/frc/robot/commands/Autonomous/OneShotSequenceMiddle.java index 5fd9602..48d89bf 100644 --- a/src/main/java/frc/robot/commands/Autonomous/OneShotSequenceMiddle.java +++ b/src/main/java/frc/robot/commands/Autonomous/OneShotSequenceMiddle.java @@ -5,6 +5,7 @@ package frc.robot.commands.Autonomous; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import frc.robot.commands.DriveCommands.MoveDistance; import frc.robot.commands.HoodCommands.MoveHoodToAngle; @@ -30,8 +31,10 @@ public OneShotSequenceMiddle(TurretSubsystem turretSubsystem, IntakeSubsystem in // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - //new MoveAndMoveHood(driveTrain, 0.4, 12, hood), - new MoveDistance(driveTrain, .4, 0.3), + new MoveAndMoveHood(driveTrain, 0.4, 0.3, hood), + //new MoveDistance(driveTrain, .4, 0.3), + new NonVisionParallelShoot(shooterSubsystem, intakeSubsystem, 12000), + new WaitCommand(1), new NonVisionParallelShoot(shooterSubsystem, intakeSubsystem, 12000), new ParallelMoveAndTurretReset(driveTrain, speed, 1.65, turretSubsystem, Constants.AUTO_TURRET_SPEED, intakeSubsystem) ); diff --git a/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java b/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java index 62c4cb3..9bbb24d 100644 --- a/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java +++ b/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java @@ -31,13 +31,13 @@ public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpee // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - //new MoveHoodToAngle(hood, 109.0), - //new NonVisionParallelShoot(shooterSubsystem, intakeSubsystem, 12000.0), + new MoveHoodToAngle(hood, 109.0), + new NonVisionParallelShoot(shooterSubsystem, intakeSubsystem, 12000.0), + new WaitCommand(1), + new NonVisionParallelShoot(shooterSubsystem, intakeSubsystem, 12000.0), new ParallelMoveAndTurretResetAndIntake(driveTrain, 0.4, 1, turretSubsystem, turretSpeed, intakeSubsystem, hood), new AutoTargetSequence(turretSubsystem, limeLightVision, hood), new ShooterSequeunce(shooterSubsystem, limeLightVision, turretSubsystem), - new WaitCommand(0.5), - new ShooterSequeunce(shooterSubsystem, limeLightVision, turretSubsystem), new PidTurnDegrees(driveTrain, -111), new ParralelMoveAndIntakeAndSetTurret(driveTrain, 0.5, 2.3, turretSubsystem, -turretSpeed, intakeSubsystem, hood, turretSubsystem), new PidTurnDegrees(driveTrain, 65), diff --git a/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceLeft.java b/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceLeft.java index b20eb10..84ca6d1 100644 --- a/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceLeft.java +++ b/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceLeft.java @@ -32,6 +32,8 @@ public TwoShotSequenceLeft(TurretSubsystem turretSubsystem, double turretSpeed, new MoveAndMoveHood(driveTrain, 0.4, 0.3, hood), //new MoveHoodToAngle(hood, 106.0), new NonVisionParallelShootDeployIntake(shooterSubsystem, intakeSubsystem, 12000), + new WaitCommand(1), + new NonVisionParallelShootDeployIntake(shooterSubsystem, intakeSubsystem, 12000), new ParallelMoveAndTurretResetAndIntake(driveTrain, speed, 1.5, turretSubsystem, turretSpeed, intakeSubsystem, hood), new AutoTargetSequence(turretSubsystem, limeLightVision, hood), //new WaitCommand(0.8), diff --git a/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceRight.java b/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceRight.java index e9b8511..f18b7ac 100644 --- a/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceRight.java +++ b/src/main/java/frc/robot/commands/Autonomous/TwoShotSequenceRight.java @@ -5,6 +5,7 @@ package frc.robot.commands.Autonomous; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.DriveCommands.MoveDistance; import frc.robot.commands.HoodCommands.MoveHoodToAngle; import frc.robot.commands.ShooterCommands.AutoTargetSequence; @@ -31,6 +32,8 @@ public TwoShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpeed, //new MoveAndMoveHood(driveTrain, speed, 12, hood), new MoveHoodToAngle(hood, 109.0), new NonVisionParallelShootDeployIntake(shooterSubsystem, intakeSubsystem, 11900), + new WaitCommand(1), + new NonVisionParallelShootDeployIntake(shooterSubsystem, intakeSubsystem, 11900), new ParallelMoveAndTurretResetAndIntake(driveTrain, 0.4, 1.0, turretSubsystem, turretSpeed, intakeSubsystem, hood), new AutoTargetSequence(turretSubsystem, limeLightVision, hood), //new WaitCommand(0.8),