diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2bf05bff..446ffd4e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import frc.robot.Constants.Presets; @@ -141,17 +142,20 @@ private void configureBindings() { _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE)); _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE)); - _operatorController.R1().whileTrue( - Commands.parallel( - new SetShooter(_shooterSubsystem, () -> Presets.SHOOTER_AMP_HANDOFF), - new SetElevator(_elevatorSubsystem, () -> Presets.ELEVATOR_AMP_HANDOFF) - ) - ); + // _operatorController.R1().whileTrue( + // Commands.parallel( + // new SetShooter(_shooterSubsystem, () -> Presets.SHOOTER_AMP_HANDOFF), + // new SetElevator(_elevatorSubsystem, () -> Presets.ELEVATOR_AMP_HANDOFF) + // ) + // ); // driver bindings _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem)); _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem)); _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem)); + _driveController.square().onTrue( + Commands.runOnce(() -> {AutonShoot.isIntaked = false; AutonShoot.isRevved = false;}) + ); // TESTING ONLY!!! _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem)); diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java index 84b3f88f..19bdebd1 100644 --- a/src/main/java/frc/robot/commands/auto/AutoAim.java +++ b/src/main/java/frc/robot/commands/auto/AutoAim.java @@ -24,7 +24,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoAim extends SequentialCommandGroup { +public class AutoAim extends ParallelCommandGroup { /** * Creates a new AutoAim. * @@ -59,11 +59,9 @@ private AutoAim( // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new ParallelCommandGroup( - new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce), - new SetShooter(shooter, shooterAngle, runOnce), - new SetElevator(elevator, elevatorHeight, runOnce) - ).beforeStarting(() -> UtilFuncs.SetLEDs(LEDState.AIM)).finallyDo(() -> UtilFuncs.SetLEDs(LEDState.DEFAULT)) + // new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce), + new SetShooter(shooter, shooterAngle, runOnce), + new SetElevator(elevator, elevatorHeight, runOnce) ); } diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java index 21681baf..c851d54f 100644 --- a/src/main/java/frc/robot/commands/auto/AutonShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutonShoot.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.intake.FeedActuate; @@ -15,6 +16,7 @@ import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; +import frc.robot.subsystems.IntakeSubsystem.ActuatorState; import frc.robot.subsystems.IntakeSubsystem.FeedMode; import frc.robot.subsystems.ShooterSubsystem.ShooterState; import frc.robot.utils.UtilFuncs; @@ -41,16 +43,12 @@ public AutonShoot( // addCommands(new FooCommand(), new BarCommand()); addCommands( new ParallelCommandGroup( - // This command will squeeze the note while revving up the shooter. It will only run if the note can't be shot right away. - new ParallelCommandGroup( - new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitCommand(2)).andThen( - () -> isRevved = true - ).onlyIf(() -> !isRevved), - new FeedActuate(intake, FeedMode.INTAKE).onlyIf(() -> !isIntaked).withTimeout(1) - ), - + new SpinShooter(shooter, ShooterState.SHOOT, true).alongWith(new WaitCommand(2)).andThen( + () -> isRevved = true + ).andThen(new PrintCommand("DONE REVVING")).onlyIf(() -> !isRevved), + // new FeedActuate(intake, ActuatorState.STOWED, FeedMode.INTAKE).withTimeout(1), new AutoAim(swerve, shooter, elevator, leds) - ), + ).andThen(new PrintCommand("DONE PRE-SHOT")), new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1), new SpinShooter(shooter, ShooterState.IDLE, true) diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 801b04a1..4f5546d8 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -232,7 +232,7 @@ else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDiff } SmartDashboard.putNumber("STD", xyStds); - System.out.println("UPDATING POSE"); + // System.out.println("UPDATING POSE"); _estimator.addVisionMeasurement(UtilFuncs.ToPose(llBotpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd)); }