diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 484ec2cb..8bf97c7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,8 +22,12 @@ import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; -import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; +import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.parallels.RunHopperAndFeeder; +import frc.robot.commands.sequences.IntakeDownSequence; +import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.RunAnglerToReverseLimit; @@ -82,6 +86,9 @@ public class RobotContainer { private final AutoChooser autoChooser = new AutoChooser(); // The robot's subsystems and commands are defined here... // private final TiltSubsystem tiltSubsystem; + private final CommandXboxController controller = + new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); + private final ClimberSubsystem climberSubsystem; private final AnglerSubsystem anglerSubsystem; private final IntakeSubsystem intakeSubsystem; private final FeederSubsystem feederSubsystem; @@ -89,7 +96,6 @@ public class RobotContainer { private final ShooterSubsystem shooterSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; - private final ClimberSubsystem climberSubsystem; private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; @@ -261,6 +267,18 @@ private void setUpAutoFactory() { } private void configureBindings() { + controller.a().onTrue(new IntakeUpSequence(intakeDeployer, intakeSubsystem)); + controller.b().onTrue(new IntakeDownSequence(intakeDeployer, intakeSubsystem)); + controller.y().onTrue(new ClimberUp(climberSubsystem)); + controller.x().onTrue(new ClimberDown(climberSubsystem)); + controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); + controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); + steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem))); + driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); + + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) // .onTrue(new ExampleCommand(m_exampleSubsystem)); @@ -280,16 +298,20 @@ private void configureBindings() { shootState); } - if (!Constants.TESTBED) { - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driveJoystick.getY() * -1, - () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(steerJoystick::getX) - .deadband(Constants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + { + if (!Constants.TESTBED) { + SwerveInputStream driveAngularVelocity = SwerveInputStream + .of(drivebase.getSwerveDrive(), + () -> driveJoystick.getY() * -1, + () -> driveJoystick.getX() * -1) + .withControllerRotationAxis(steerJoystick::getX) + .deadband(Constants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + Command driveFieldOrientedAnglularVelocity = drivebase + .driveFieldOriented(driveAngularVelocity); + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + } } } @@ -329,6 +351,9 @@ public void putShuffleboardCommands() { SmartDashboard.putNumber("angler/TargetAngle", 0); + SmartDashboard.putData("RunHoppperAndFeeder", + new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); + SmartDashboard.putData( "angler/Set Position", new InstantCommand(() -> anglerSubsystem.setPosition( @@ -394,26 +419,26 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "intakedeployer/InitlizeDeployer", - new InitalRunDeployment(intakeDeployer)); - SmartDashboard.putData( - "Spin Intake", - new SpinIntake(intakeSubsystem)); + new InitialRunDeployment(intakeDeployer)); + SmartDashboard.putData( + "Spin Intake", + new SpinIntake(intakeSubsystem)); - SmartDashboard.putData( - "Start Hopper", - new SpinHopper(hopperSubsystem)); + SmartDashboard.putData( + "Start Hopper", + new SpinHopper(hopperSubsystem)); - SmartDashboard.putData( - "Climber Up", - new ClimberUp(climberSubsystem)); + SmartDashboard.putData( + "Climber Up", + new ClimberUp(climberSubsystem)); - SmartDashboard.putData( - "Climber Down", - new ClimberDown(climberSubsystem)); + SmartDashboard.putData( + "Climber Down", + new ClimberDown(climberSubsystem)); - SmartDashboard.putData( - "Spin Feeder", - new SpinFeeder(feederSubsystem)); + SmartDashboard.putData( + "Spin Feeder", + new SpinFeeder(feederSubsystem)); SmartDashboard.putData( "Spin Shooter", diff --git a/src/main/java/frc/robot/commands/intake/StopIntake.java b/src/main/java/frc/robot/commands/intake/StopIntake.java new file mode 100644 index 00000000..c9f4df29 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/StopIntake.java @@ -0,0 +1,31 @@ +package frc.robot.commands.intake; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class StopIntake extends LoggableCommand { + + private final IntakeSubsystem subsystem; + + public StopIntake(IntakeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java similarity index 90% rename from src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java rename to src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java index 0d72ff69..7de4143f 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java @@ -11,11 +11,11 @@ /** * When creating sequences use this AFTER setting the deployment state */ -public class InitalRunDeployment extends LoggableCommand { +public class InitialRunDeployment extends LoggableCommand { private final IntakeDeployerSubsystem subsystem; private final Timer timer; - public InitalRunDeployment(IntakeDeployerSubsystem subsystem) { + public InitialRunDeployment(IntakeDeployerSubsystem subsystem) { timer = new Timer(); this.subsystem = subsystem; addRequirements(subsystem); diff --git a/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java b/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java new file mode 100644 index 00000000..f515e6a9 --- /dev/null +++ b/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java @@ -0,0 +1,19 @@ +package frc.robot.commands.parallels; + +import frc.robot.commands.intake.SpinIntake; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; + +public class DeployAndSpinIntake extends LoggableParallelCommandGroup{ + + public DeployAndSpinIntake(IntakeDeployerSubsystem deployer, IntakeSubsystem intakeSubsystem){ + + super( + new InitialRunDeployment(deployer), + new SpinIntake(intakeSubsystem) + + ); + } +} diff --git a/src/main/java/frc/robot/commands/parallels/RunHopperAndFeeder.java b/src/main/java/frc/robot/commands/parallels/RunHopperAndFeeder.java new file mode 100644 index 00000000..40ea0865 --- /dev/null +++ b/src/main/java/frc/robot/commands/parallels/RunHopperAndFeeder.java @@ -0,0 +1,19 @@ +package frc.robot.commands.parallels; + +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; + +public class RunHopperAndFeeder extends LoggableParallelCommandGroup{ + + public RunHopperAndFeeder(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem){ + + super( + new SpinHopper(hopperSubsystem), + new SpinFeeder(feederSubsystem) + + ); + } +} diff --git a/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java b/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java new file mode 100644 index 00000000..63351995 --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java @@ -0,0 +1,19 @@ +package frc.robot.commands.sequences; + +import frc.robot.commands.intake.SpinIntake; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; +import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.parallels.DeployAndSpinIntake; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class IntakeDownSequence extends LoggableSequentialCommandGroup{ + public IntakeDownSequence(IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem){ + super( + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.DOWN), + new DeployAndSpinIntake(intakeDeployerSubsystem, intakeSubsystem) + ); + } +} diff --git a/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java b/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java new file mode 100644 index 00000000..bfaa673e --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java @@ -0,0 +1,18 @@ +package frc.robot.commands.sequences; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; +import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; + +public class IntakeUpSequence extends LoggableSequentialCommandGroup{ + public IntakeUpSequence(IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem){ + super( + new StopIntake(intakeSubsystem), + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new InitialRunDeployment(intakeDeployerSubsystem) + ); + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index fab3f92d..ac3ac2c3 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -61,9 +61,9 @@ public enum Mode { //Timeouts public static final double SPIN_TIMEOUT = 5; public static final double TILT_TIMEOUT = 5; - public static final double HOPPER_TIMEOUT = 10; + public static final double HOPPER_TIMEOUT = 60; public static final double CLIMBER_TIMEOUT = 10; - public static final double FEEDER_TIMEOUT = 3; + public static final double FEEDER_TIMEOUT = 60; public static final double ANGLER_TIMEOUT = 5; public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; public static final double SHOOTER_TIMEOUT = 5;