From 2e6f589559c5910040e984496ea5bd728978da01 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Mon, 26 Feb 2024 18:22:25 -0800 Subject: [PATCH 01/15] Added rumble to drive controller driver controller rumbles when feeder sensor senses note and intake stops --- .../java/frc/team2412/robot/Controls.java | 11 ++++++++-- .../robot/commands/intake/AllInCommand.java | 6 ++++- .../commands/intake/StopRumbleCommand.java | 22 +++++++++++++++++++ 3 files changed, 36 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 52a528be..d6a0fcbc 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -11,7 +11,9 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team2412.robot.commands.intake.AllInCommand; @@ -19,6 +21,7 @@ import frc.team2412.robot.commands.intake.AllStopCommand; import frc.team2412.robot.commands.intake.FeederInCommand; import frc.team2412.robot.commands.intake.IntakeRejectCommand; +import frc.team2412.robot.commands.intake.StopRumbleCommand; import frc.team2412.robot.commands.launcher.FullTargetCommand; import frc.team2412.robot.commands.launcher.SetAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; @@ -122,11 +125,15 @@ private void bindDrivebaseControls() { private void bindIntakeControls() { // CommandScheduler.getInstance() // .setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem)); - driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem)); + driveIntakeInButton.onTrue( + Commands.sequence( + new AllInCommand(s.intakeSubsystem, this), + new WaitCommand(0.5), + new StopRumbleCommand(this))); driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); driveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); - codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem)); + codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); codriveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); codriveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); codriveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index a5e26f04..0215a75e 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -1,13 +1,16 @@ package frc.team2412.robot.commands.intake; import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.Controls; import frc.team2412.robot.subsystems.IntakeSubsystem; public class AllInCommand extends Command { private final IntakeSubsystem intakeSubsystem; + private final Controls controls; - public AllInCommand(IntakeSubsystem intakeSubsystem) { + public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { this.intakeSubsystem = intakeSubsystem; + this.controls = controls; addRequirements(intakeSubsystem); } @@ -30,6 +33,7 @@ public void end(boolean interrupted) { intakeSubsystem.intakeReject(); intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); + controls.vibrateDriveController(1.0); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java new file mode 100644 index 00000000..fee8000a --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java @@ -0,0 +1,22 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.Controls; + +public class StopRumbleCommand extends Command { + private final Controls controls; + + public StopRumbleCommand(Controls controls) { + this.controls = controls; + } + + @Override + public void initialize() { + controls.vibrateDriveController(0.0); + } + + @Override + public boolean isFinished() { + return true; + } +} From 102095b6670ac9181a8506b61f905bdd2d9d9c31 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Mon, 26 Feb 2024 18:37:43 -0800 Subject: [PATCH 02/15] merge conflicts :C --- src/main/java/frc/team2412/robot/util/AutoLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 2b09dc26..f24fae82 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -65,7 +65,7 @@ public void registerCommands() { .andThen(new WaitCommand(1)) .andThen(new FeederInCommand(s.intakeSubsystem))); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem)); + NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, controls)); // Complex Autos NamedCommands.registerCommand("AutoLogicTest", AutoPaths.testAuto); NamedCommands.registerCommand( From 96f9025b80d5e4f29443a60022bdbc0ff12dcf2f Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 27 Feb 2024 17:52:10 -0800 Subject: [PATCH 03/15] Changed command structure for rumble addressing comments by adding AllInRumbleCommand --- .../java/frc/team2412/robot/Controls.java | 13 +++-------- .../commands/intake/AllInRumbleCommand.java | 23 +++++++++++++++++++ .../commands/intake/StopRumbleCommand.java | 22 ------------------ .../robot/subsystems/IntakeSubsystem.java | 1 + 4 files changed, 27 insertions(+), 32 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java delete mode 100644 src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 64101497..16c80044 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -11,17 +11,14 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team2412.robot.commands.intake.AllInCommand; +import frc.team2412.robot.commands.intake.AllInRumbleCommand; import frc.team2412.robot.commands.intake.AllReverseCommand; import frc.team2412.robot.commands.intake.AllStopCommand; import frc.team2412.robot.commands.intake.FeederInCommand; import frc.team2412.robot.commands.intake.IntakeRejectCommand; -import frc.team2412.robot.commands.intake.StopRumbleCommand; import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; import frc.team2412.robot.commands.launcher.SetAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; @@ -124,15 +121,11 @@ private void bindDrivebaseControls() { private void bindIntakeControls() { // CommandScheduler.getInstance() // .setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem)); - driveIntakeInButton.onTrue( - Commands.sequence( - new AllInCommand(s.intakeSubsystem, this), - new WaitCommand(0.5), - new StopRumbleCommand(this))); + driveIntakeInButton.onTrue(new AllInRumbleCommand(s.intakeSubsystem, this)); driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); driveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); - codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); + codriveIntakeInButton.onTrue(new AllInRumbleCommand(s.intakeSubsystem, this)); codriveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); codriveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); codriveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java new file mode 100644 index 00000000..87771db6 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java @@ -0,0 +1,23 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.team2412.robot.Controls; +import frc.team2412.robot.subsystems.IntakeSubsystem; + +public class AllInRumbleCommand extends SequentialCommandGroup { + private final IntakeSubsystem intakeSubsystem; + private final Controls controls; + + public AllInRumbleCommand(IntakeSubsystem intakeSubsystem, Controls controls) { + this.intakeSubsystem = intakeSubsystem; + this.controls = controls; + addRequirements(intakeSubsystem); + + addCommands(new AllInCommand(intakeSubsystem, controls), new WaitCommand(0.5)); + } + + public void end() { + controls.vibrateDriveController(0.0); + } +} diff --git a/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java deleted file mode 100644 index fee8000a..00000000 --- a/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.team2412.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.team2412.robot.Controls; - -public class StopRumbleCommand extends Command { - private final Controls controls; - - public StopRumbleCommand(Controls controls) { - this.controls = controls; - } - - @Override - public void initialize() { - controls.vibrateDriveController(0.0); - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 02981ceb..a944e605 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -127,6 +127,7 @@ public class IntakeSubsystem extends SubsystemBase { .getEntry(); public IntakeSubsystem() { + intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); intakeMotorBack = new CANSparkMax(INTAKE_MOTOR_BACK, MotorType.kBrushless); intakeMotorLeft = new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless); From 5be5392ebd7ad2162b30bcfafd7d13862ff8013f Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 27 Feb 2024 18:04:43 -0800 Subject: [PATCH 04/15] Stop intake rumble in auto --- .../frc/team2412/robot/commands/intake/AllInCommand.java | 5 ++++- src/main/java/frc/team2412/robot/util/AutoLogic.java | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 0215a75e..226663e1 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -33,7 +33,10 @@ public void end(boolean interrupted) { intakeSubsystem.intakeReject(); intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); - controls.vibrateDriveController(1.0); + + if (controls != null && interrupted == false) { + controls.vibrateDriveController(1.0); + } } @Override diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index f24fae82..91a70dae 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -65,7 +65,7 @@ public void registerCommands() { .andThen(new WaitCommand(1)) .andThen(new FeederInCommand(s.intakeSubsystem))); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, controls)); + NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); // Complex Autos NamedCommands.registerCommand("AutoLogicTest", AutoPaths.testAuto); NamedCommands.registerCommand( From ba6737878037367581f3511f8c70aad49dfab0e5 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 27 Feb 2024 18:06:40 -0800 Subject: [PATCH 05/15] deleting addRequirements in AllInRumbleCommand --- .../frc/team2412/robot/commands/intake/AllInRumbleCommand.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java index 87771db6..eab475e2 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java @@ -12,7 +12,6 @@ public class AllInRumbleCommand extends SequentialCommandGroup { public AllInRumbleCommand(IntakeSubsystem intakeSubsystem, Controls controls) { this.intakeSubsystem = intakeSubsystem; this.controls = controls; - addRequirements(intakeSubsystem); addCommands(new AllInCommand(intakeSubsystem, controls), new WaitCommand(0.5)); } From e0167e9601180b9eeabe69c46c4ae334761709da Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 27 Feb 2024 18:30:56 -0800 Subject: [PATCH 06/15] Added AllInNoSensorCommand intaking without sensors for auto usage --- .../commands/intake/AllInNoSensorCommand.java | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java new file mode 100644 index 00000000..17e459e9 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java @@ -0,0 +1,25 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.IntakeSubsystem; + +public class AllInNoSensorCommand extends Command { + private final IntakeSubsystem intakeSubsystem; + + public AllInNoSensorCommand(IntakeSubsystem intakeSubsystem) { + this.intakeSubsystem = intakeSubsystem; + addRequirements(intakeSubsystem); + } + + @Override + public void initialize() { + intakeSubsystem.intakeIn(); + intakeSubsystem.indexIn(); + intakeSubsystem.feederIn(); + } + + @Override + public boolean isFinished() { + return true; + } +} From 2ee77936a90dcfbe6d275d38355fda913f805332 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 27 Feb 2024 19:05:34 -0800 Subject: [PATCH 07/15] rewrote interrupted in AllInCommand --- .../java/frc/team2412/robot/commands/intake/AllInCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 226663e1..de4ae62c 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -34,7 +34,7 @@ public void end(boolean interrupted) { intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); - if (controls != null && interrupted == false) { + if (controls != null && !interrupted) { controls.vibrateDriveController(1.0); } } From 9d5083b8bb37b3b8379500712183c14abc95d01d Mon Sep 17 00:00:00 2001 From: Yumnah Date: Thu, 7 Mar 2024 19:13:50 -0800 Subject: [PATCH 08/15] stop rumble command ignore this actually --- .../commands/intake/AllInRumbleCommand.java | 9 ++++--- .../commands/intake/StopRumbleCommand.java | 26 +++++++++++++++++++ 2 files changed, 32 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java index eab475e2..8a086f7a 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java @@ -16,7 +16,10 @@ public AllInRumbleCommand(IntakeSubsystem intakeSubsystem, Controls controls) { addCommands(new AllInCommand(intakeSubsystem, controls), new WaitCommand(0.5)); } - public void end() { - controls.vibrateDriveController(0.0); - } + @Override + public final void end (boolean interrupted) { + if (interrupted){ + controls.vibrateDriveController(0.0); + } + } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java new file mode 100644 index 00000000..91c0479d --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java @@ -0,0 +1,26 @@ +package frc.team2412.robot.commands.intake; +import frc.team2412.robot.Controls; +import frc.team2412.robot.subsystems.IntakeSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +public class StopRumbleCommand extends Command { + private final IntakeSubsystem intakeSubsystem; + private final Controls controls; + + public StopRumbleCommand(IntakeSubsystem intakeSubsystem, Controls controls){ + this.intakeSubsystem = intakeSubsystem; + this.controls = controls; + } + + + @Override + public void initialize() { + controls.vibrateDriveController(0.0); + } + + @Override + public boolean isFinished() { + return true; + } + +} From 8266e89057cce1e9c2f8be9db0bf32cd30b67f4d Mon Sep 17 00:00:00 2001 From: Yumnah Date: Sat, 9 Mar 2024 10:48:11 -0800 Subject: [PATCH 09/15] Rumble and intake sensor logic All rollers reject after intake sensor logic and we included rumble through different stages --- .../java/frc/team2412/robot/Controls.java | 6 +-- .../robot/commands/intake/AllInCommand.java | 44 +++++++++++++++++-- .../commands/intake/AllInRumbleCommand.java | 25 ----------- .../robot/commands/intake/RumbleCommand.java | 27 ++++++++++++ .../commands/intake/StopRumbleCommand.java | 26 ----------- .../robot/subsystems/IntakeSubsystem.java | 43 ++++++++++++++++++ .../frc/team2412/robot/util/AutoLogic.java | 2 +- 7 files changed, 114 insertions(+), 59 deletions(-) delete mode 100644 src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java create mode 100644 src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java delete mode 100644 src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 2876b6e6..5fa64e77 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team2412.robot.commands.intake.AllInRumbleCommand; +import frc.team2412.robot.commands.intake.AllInCommand; import frc.team2412.robot.commands.intake.AllReverseCommand; import frc.team2412.robot.commands.intake.AllStopCommand; import frc.team2412.robot.commands.intake.FeederInCommand; @@ -122,12 +122,12 @@ private void bindDrivebaseControls() { private void bindIntakeControls() { // CommandScheduler.getInstance() // .setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem)); - driveIntakeInButton.onTrue(new AllInRumbleCommand(s.intakeSubsystem, this)); + driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); driveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); - codriveIntakeInButton.onTrue(new AllInRumbleCommand(s.intakeSubsystem, this)); + codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); codriveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); codriveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); codriveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 447c7292..f23d4687 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -1,6 +1,8 @@ package frc.team2412.robot.commands.intake; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team2412.robot.Controls; import frc.team2412.robot.subsystems.IntakeSubsystem; @@ -23,20 +25,54 @@ public void initialize() { @Override public void execute() { + // intake rejecting + if (intakeSubsystem.intakeFrontSeesNote()) { + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeLeftReject(); + intakeSubsystem.intakeRightReject(); + + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } + + if (intakeSubsystem.intakeBackSeesNote()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeLeftReject(); + intakeSubsystem.intakeRightReject(); + + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } + + if (intakeSubsystem.intakeLeftSeesNote()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeRightReject(); + + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } + + if (intakeSubsystem.intakeRightSeesNote()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeLeftReject(); + + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } + + // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { intakeSubsystem.intakeReject(); + + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); } } @Override public void end(boolean interrupted) { - // intakeSubsystem.intakeReject(); + intakeSubsystem.intakeReject(); intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); - if (controls != null && !interrupted) { - controls.vibrateDriveController(1.0); - } + Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java deleted file mode 100644 index 8a086f7a..00000000 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInRumbleCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.team2412.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team2412.robot.Controls; -import frc.team2412.robot.subsystems.IntakeSubsystem; - -public class AllInRumbleCommand extends SequentialCommandGroup { - private final IntakeSubsystem intakeSubsystem; - private final Controls controls; - - public AllInRumbleCommand(IntakeSubsystem intakeSubsystem, Controls controls) { - this.intakeSubsystem = intakeSubsystem; - this.controls = controls; - - addCommands(new AllInCommand(intakeSubsystem, controls), new WaitCommand(0.5)); - } - - @Override - public final void end (boolean interrupted) { - if (interrupted){ - controls.vibrateDriveController(0.0); - } - } -} diff --git a/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java new file mode 100644 index 00000000..4165bb0e --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java @@ -0,0 +1,27 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.Controls; + +public class RumbleCommand extends Command { + private final Controls controls; + + public RumbleCommand(Controls controls) { + this.controls = controls; + } + + @Override + public void initialize() { + controls.vibrateDriveController(1.0); + } + + @Override + public void end(boolean interrupted) { + controls.vibrateDriveController(0.0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java deleted file mode 100644 index 91c0479d..00000000 --- a/src/main/java/frc/team2412/robot/commands/intake/StopRumbleCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.team2412.robot.commands.intake; -import frc.team2412.robot.Controls; -import frc.team2412.robot.subsystems.IntakeSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -public class StopRumbleCommand extends Command { - private final IntakeSubsystem intakeSubsystem; - private final Controls controls; - - public StopRumbleCommand(IntakeSubsystem intakeSubsystem, Controls controls){ - this.intakeSubsystem = intakeSubsystem; - this.controls = controls; - } - - - @Override - public void initialize() { - controls.vibrateDriveController(0.0); - } - - @Override - public boolean isFinished() { - return true; - } - -} diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 4ea894b6..21db65e1 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -7,6 +7,7 @@ import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkLimitSwitch; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -47,6 +48,11 @@ public class IntakeSubsystem extends SubsystemBase { private final DigitalInput indexSensor; private final DigitalInput feederSensor; + private final SparkLimitSwitch intakeFrontSensor; + private final SparkLimitSwitch intakeBackSensor; + private final SparkLimitSwitch intakeLeftSensor; + private final SparkLimitSwitch intakeRightSensor; + // Shuffleboard // speed private final GenericEntry setIntakeInSpeedEntry = @@ -141,6 +147,11 @@ public IntakeSubsystem() { indexSensor = new DigitalInput(INDEX_SENSOR); feederSensor = new DigitalInput(FEEDER_SENSOR); + intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeBackSensor = intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + resetMotors(); ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); @@ -198,6 +209,22 @@ public void intakeReject() { intakeSet(INTAKE_REJECT_SPEED); } + public void intakeFrontReject() { + intakeMotorFront.set(INTAKE_REJECT_SPEED); + } + + public void intakeBackReject() { + intakeMotorBack.set(INTAKE_REJECT_SPEED); + } + + public void intakeLeftReject() { + intakeMotorLeft.set(INTAKE_REJECT_SPEED); + } + + public void intakeRightReject() { + intakeMotorRight.set(INTAKE_REJECT_SPEED); + } + // index methods public void indexIn() { indexMotorUpper.set(setIndexInSpeedEntry.getDouble(INDEX_UPPER_IN_SPEED)); @@ -238,6 +265,22 @@ public boolean feederSensorHasNote() { return !feederSensor.get() && !getSensorOverride(); } + public boolean intakeFrontSeesNote() { + return intakeFrontSensor.isPressed(); + } + + public boolean intakeBackSeesNote() { + return intakeBackSensor.isPressed(); + } + + public boolean intakeLeftSeesNote() { + return intakeLeftSensor.isPressed(); + } + + public boolean intakeRightSeesNote() { + return intakeRightSensor.isPressed(); + } + public boolean getSensorOverride() { return sensorOverride.getBoolean(false); } diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 62f07227..28b1eea0 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -52,7 +52,7 @@ public void registerCommands() { // Intake NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem)); + NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, controls)); NamedCommands.registerCommand( "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); // Launcher From bfcea666266d7117087fab08e89b425254ac4ae2 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Sat, 9 Mar 2024 10:54:11 -0800 Subject: [PATCH 10/15] removed rumble during auto no rumbling during auto --- .../robot/commands/intake/AllInCommand.java | 24 ++++++++++++++----- .../frc/team2412/robot/util/AutoLogic.java | 2 +- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index f23d4687..cab68e63 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -31,7 +31,9 @@ public void execute() { intakeSubsystem.intakeLeftReject(); intakeSubsystem.intakeRightReject(); - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } } if (intakeSubsystem.intakeBackSeesNote()) { @@ -39,7 +41,9 @@ public void execute() { intakeSubsystem.intakeLeftReject(); intakeSubsystem.intakeRightReject(); - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } } if (intakeSubsystem.intakeLeftSeesNote()) { @@ -47,7 +51,9 @@ public void execute() { intakeSubsystem.intakeBackReject(); intakeSubsystem.intakeRightReject(); - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } } if (intakeSubsystem.intakeRightSeesNote()) { @@ -55,14 +61,18 @@ public void execute() { intakeSubsystem.intakeBackReject(); intakeSubsystem.intakeLeftReject(); - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + } } // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { intakeSubsystem.intakeReject(); - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + } } } @@ -72,7 +82,9 @@ public void end(boolean interrupted) { intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + } } @Override diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 28b1eea0..d6ea0b3d 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -52,7 +52,7 @@ public void registerCommands() { // Intake NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, controls)); + NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); NamedCommands.registerCommand( "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); // Launcher From afd24bff5b878541e3178ef70d01d56f5a948ea7 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 12 Mar 2024 09:11:32 -0700 Subject: [PATCH 11/15] added intake Reject override, rumble debugging rumble debugging- rumbles during auto (will fix that later), increased rumble time, rumble to both controllers --- .../java/frc/team2412/robot/Controls.java | 12 ++-- .../java/frc/team2412/robot/Subsystems.java | 4 +- .../robot/commands/intake/AllInCommand.java | 72 +++++++++++++------ .../commands/launcher/FullTargetCommand.java | 6 +- .../robot/subsystems/IntakeSubsystem.java | 36 +++++++++- 5 files changed, 100 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 5fa64e77..039a6c35 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -19,6 +18,7 @@ import frc.team2412.robot.commands.intake.AllStopCommand; import frc.team2412.robot.commands.intake.FeederInCommand; import frc.team2412.robot.commands.intake.IntakeRejectCommand; +import frc.team2412.robot.commands.intake.RumbleCommand; import frc.team2412.robot.commands.launcher.FullTargetCommand; import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; import frc.team2412.robot.commands.launcher.SetAngleCommand; @@ -122,7 +122,7 @@ private void bindDrivebaseControls() { private void bindIntakeControls() { // CommandScheduler.getInstance() // .setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem)); - driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); + driveIntakeInButton.onTrue(new RumbleCommand(this)); driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); @@ -177,8 +177,10 @@ private void bindLauncherControls() { } public void vibrateDriveController(double vibration) { - if (!DriverStation.isAutonomous()) { - driveController.getHID().setRumble(RumbleType.kBothRumble, vibration); - } + // if (!DriverStation.isAutonomous()) { + + // } + driveController.getHID().setRumble(RumbleType.kBothRumble, vibration); + codriveController.getHID().setRumble(RumbleType.kBothRumble, vibration); } } diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index c71fbdd1..c5b39973 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -14,8 +14,8 @@ public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = true; - public static final boolean LIMELIGHT_ENABLED = false; - public static final boolean CLIMB_ENABLED = false; + public static final boolean LIMELIGHT_ENABLED = true; + public static final boolean CLIMB_ENABLED = true; public static final boolean LAUNCHER_ENABLED = true; public static final boolean INTAKE_ENABLED = true; public static final boolean DRIVEBASE_ENABLED = true; diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index cab68e63..57a0f6ae 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -27,63 +27,95 @@ public void initialize() { public void execute() { // intake rejecting if (intakeSubsystem.intakeFrontSeesNote()) { - intakeSubsystem.intakeBackReject(); - intakeSubsystem.intakeLeftReject(); - intakeSubsystem.intakeRightReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeLeftReject(); + intakeSubsystem.intakeRightReject(); + } else { + intakeSubsystem.intakeBackStop(); + intakeSubsystem.intakeLeftStop(); + intakeSubsystem.intakeRightStop(); + } if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); } } if (intakeSubsystem.intakeBackSeesNote()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeLeftReject(); - intakeSubsystem.intakeRightReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeLeftReject(); + intakeSubsystem.intakeRightReject(); + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeLeftStop(); + intakeSubsystem.intakeRightStop(); + } if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); } } if (intakeSubsystem.intakeLeftSeesNote()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeBackReject(); - intakeSubsystem.intakeRightReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeRightReject(); + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeBackStop(); + intakeSubsystem.intakeRightStop(); + } if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); } } if (intakeSubsystem.intakeRightSeesNote()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeBackReject(); - intakeSubsystem.intakeLeftReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeLeftReject(); + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeBackStop(); + intakeSubsystem.intakeLeftStop(); + } if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(0.5)).schedule(); + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); } } // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { - intakeSubsystem.intakeReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeReject(); + } else { + intakeSubsystem.intakeStop(); + } if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); } } } @Override public void end(boolean interrupted) { - intakeSubsystem.intakeReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeReject(); + } else { + intakeSubsystem.intakeStop(); + } intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); } } diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index 7634d431..e7edeaa6 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -1,5 +1,7 @@ package frc.team2412.robot.commands.launcher; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -38,7 +40,9 @@ public FullTargetCommand( this.launcherSubsystem = launcherSubsystem; this.drivebaseSubsystem = drivebaseSubsystem; this.controls = controls; - yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false); + if (DRIVEBASE_ENABLED) { + yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false); + } addRequirements(launcherSubsystem); } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 21db65e1..ec4f6077 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -54,7 +54,7 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkLimitSwitch intakeRightSensor; // Shuffleboard - // speed + // control intake speed private final GenericEntry setIntakeInSpeedEntry = Shuffleboard.getTab("Intake") .addPersistent("Intake in speed - ", INTAKE_IN_SPEED) @@ -74,7 +74,7 @@ public class IntakeSubsystem extends SubsystemBase { .withSize(1, 1) .getEntry(); - // temperature + // motor temperature private final GenericEntry intakeMotorFrontTemp = Shuffleboard.getTab("Intake") .add("Front Intake temp", 0) @@ -132,6 +132,14 @@ public class IntakeSubsystem extends SubsystemBase { .withWidget(BuiltInWidgets.kToggleSwitch) .getEntry(); + // reject override + private final GenericEntry rejectOverride = + Shuffleboard.getTab("Intake") + .add("Override Intake Reject", false) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); + public IntakeSubsystem() { intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); @@ -157,6 +165,7 @@ public IntakeSubsystem() { ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Sensor - ", this::intakeBackSeesNote).withSize(1, 1); } private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) { @@ -201,10 +210,28 @@ public void intakeReverse() { intakeSet(INTAKE_REVERSE_SPEED); } + // intake stop methods public void intakeStop() { intakeSet(0); } + public void intakeFrontStop() { + intakeMotorFront.set(0); + } + + public void intakeBackStop() { + intakeMotorBack.set(0); + } + + public void intakeLeftStop() { + intakeMotorLeft.set(0); + } + + public void intakeRightStop() { + intakeMotorRight.set(0); + } + + // intake reject methods public void intakeReject() { intakeSet(INTAKE_REJECT_SPEED); } @@ -281,10 +308,15 @@ public boolean intakeRightSeesNote() { return intakeRightSensor.isPressed(); } + // override methods on shuffleboard public boolean getSensorOverride() { return sensorOverride.getBoolean(false); } + public boolean getRejectOverride() { + return rejectOverride.getBoolean(false); + } + @Override public void periodic() { intakeMotorFrontTemp.setDouble(intakeMotorFront.getMotorTemperature()); From 4eef46e52660ad0a298b1016694ed97430767468 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Sun, 17 Mar 2024 16:48:58 -0700 Subject: [PATCH 12/15] addressed github comments added intake sensors to shuffleboard, removed intake back sensor, changed motor configuration and stopped continuous rumbling --- .../java/frc/team2412/robot/Subsystems.java | 4 +-- .../robot/commands/intake/AllInCommand.java | 35 ++++++++----------- .../robot/subsystems/IntakeSubsystem.java | 19 ++++++---- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index c5b39973..c71fbdd1 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -14,8 +14,8 @@ public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = true; - public static final boolean LIMELIGHT_ENABLED = true; - public static final boolean CLIMB_ENABLED = true; + public static final boolean LIMELIGHT_ENABLED = false; + public static final boolean CLIMB_ENABLED = false; public static final boolean LAUNCHER_ENABLED = true; public static final boolean INTAKE_ENABLED = true; public static final boolean DRIVEBASE_ENABLED = true; diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 57a0f6ae..f5b950f7 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -9,6 +9,10 @@ public class AllInCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final Controls controls; + private boolean rumbledIntakeFront = false; + private boolean rumbledIntakeLeft = false; + private boolean rumbledIntakeRight = false; + private boolean rumbledIndex = false; public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { this.intakeSubsystem = intakeSubsystem; @@ -37,27 +41,14 @@ public void execute() { intakeSubsystem.intakeRightStop(); } - if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); - } - } - - if (intakeSubsystem.intakeBackSeesNote()) { - if (!intakeSubsystem.getRejectOverride()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeLeftReject(); - intakeSubsystem.intakeRightReject(); - } else { - intakeSubsystem.intakeFrontStop(); - intakeSubsystem.intakeLeftStop(); - intakeSubsystem.intakeRightStop(); - } - - if (controls != null) { + if (controls != null && !rumbledIntakeFront) { Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeFront = true; } } + // back intake sensor does not exist ._. + if (intakeSubsystem.intakeLeftSeesNote()) { if (!intakeSubsystem.getRejectOverride()) { intakeSubsystem.intakeFrontReject(); @@ -69,8 +60,9 @@ public void execute() { intakeSubsystem.intakeRightStop(); } - if (controls != null) { + if (controls != null && !rumbledIntakeLeft) { Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeLeft = true; } } @@ -85,8 +77,9 @@ public void execute() { intakeSubsystem.intakeLeftStop(); } - if (controls != null) { + if (controls != null && !rumbledIntakeRight) { Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeRight = true; } } @@ -98,8 +91,9 @@ public void execute() { intakeSubsystem.intakeStop(); } - if (controls != null) { + if (controls != null && !rumbledIndex) { Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + rumbledIndex = true; } } } @@ -111,6 +105,7 @@ public void end(boolean interrupted) { } else { intakeSubsystem.intakeStop(); } + intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index ec4f6077..a1717db7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -49,7 +49,7 @@ public class IntakeSubsystem extends SubsystemBase { private final DigitalInput feederSensor; private final SparkLimitSwitch intakeFrontSensor; - private final SparkLimitSwitch intakeBackSensor; + // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; private final SparkLimitSwitch intakeRightSensor; @@ -156,16 +156,25 @@ public IntakeSubsystem() { feederSensor = new DigitalInput(FEEDER_SENSOR); intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeBackSensor = intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeBackSensor = + // intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeFrontSensor.enableLimitSwitch(false); + intakeLeftSensor.enableLimitSwitch(false); + intakeRightSensor.enableLimitSwitch(false); + resetMotors(); + // show if sensors have notes in shuffleboard ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); - shuffleboardTab.addBoolean("Intake Sensor - ", this::intakeBackSeesNote).withSize(1, 1); + // intake back sensor + shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Left Sensor - ", this::intakeLeftSeesNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Right Sensor - ", this::intakeRightSeesNote).withSize(1, 1); } private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) { @@ -296,10 +305,6 @@ public boolean intakeFrontSeesNote() { return intakeFrontSensor.isPressed(); } - public boolean intakeBackSeesNote() { - return intakeBackSensor.isPressed(); - } - public boolean intakeLeftSeesNote() { return intakeLeftSensor.isPressed(); } From 2c47282cedd1741aee7a57d89148887634980916 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Mon, 18 Mar 2024 18:16:21 -0700 Subject: [PATCH 13/15] reset rumbled variables to false after intaking --- .../frc/team2412/robot/commands/intake/AllInCommand.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index f5b950f7..9152862f 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -47,7 +47,7 @@ public void execute() { } } - // back intake sensor does not exist ._. + // back intake sensor does not exist (but it might?) if (intakeSubsystem.intakeLeftSeesNote()) { if (!intakeSubsystem.getRejectOverride()) { @@ -112,6 +112,11 @@ public void end(boolean interrupted) { if (controls != null) { Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); } + + rumbledIntakeFront = false; + rumbledIntakeLeft = false; + rumbledIntakeRight = false; + rumbledIndex = false; } @Override From 6a30f2f3e7c6adb9cd0e289ae934e86af3b30d3d Mon Sep 17 00:00:00 2001 From: Yumnah Date: Mon, 18 Mar 2024 18:52:57 -0700 Subject: [PATCH 14/15] undid the rumble debugging code --- src/main/java/frc/team2412/robot/Controls.java | 12 ++++++------ .../team2412/robot/subsystems/IntakeSubsystem.java | 2 ++ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 039a6c35..18de1d06 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -122,7 +123,7 @@ private void bindDrivebaseControls() { private void bindIntakeControls() { // CommandScheduler.getInstance() // .setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem)); - driveIntakeInButton.onTrue(new RumbleCommand(this)); + driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); @@ -177,10 +178,9 @@ private void bindLauncherControls() { } public void vibrateDriveController(double vibration) { - // if (!DriverStation.isAutonomous()) { - - // } - driveController.getHID().setRumble(RumbleType.kBothRumble, vibration); - codriveController.getHID().setRumble(RumbleType.kBothRumble, vibration); + if (!DriverStation.isAutonomous()) { + driveController.getHID().setRumble(RumbleType.kBothRumble, vibration); + codriveController.getHID().setRumble(RumbleType.kBothRumble, vibration); + } } } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index a1717db7..8caa33c6 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -161,6 +161,8 @@ public IntakeSubsystem() { intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + + //todo: MOVE THIS TO CONFIGURE MOTOR intakeFrontSensor.enableLimitSwitch(false); intakeLeftSensor.enableLimitSwitch(false); intakeRightSensor.enableLimitSwitch(false); From 97971aad3f107c0bd74d5b04e43bd20296798042 Mon Sep 17 00:00:00 2001 From: jamesdooley4 <56771024+jamesdooley4@users.noreply.github.com> Date: Mon, 18 Mar 2024 19:27:43 -0700 Subject: [PATCH 15/15] Fix conflicts with main --- src/main/java/frc/team2412/robot/Controls.java | 1 - .../frc/team2412/robot/subsystems/IntakeSubsystem.java | 7 +++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 90bb4123..1a270763 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -20,7 +20,6 @@ import frc.team2412.robot.commands.intake.AllStopCommand; import frc.team2412.robot.commands.intake.FeederInCommand; import frc.team2412.robot.commands.intake.IntakeRejectCommand; -import frc.team2412.robot.commands.intake.RumbleCommand; import frc.team2412.robot.commands.launcher.FullTargetCommand; import frc.team2412.robot.commands.launcher.ManualAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index fd51da5d..6426a8ba 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -91,8 +91,7 @@ public IntakeSubsystem() { intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - - //todo: MOVE THIS TO CONFIGURE MOTOR + // todo: MOVE THIS TO CONFIGURE MOTOR intakeFrontSensor.enableLimitSwitch(false); intakeLeftSensor.enableLimitSwitch(false); intakeRightSensor.enableLimitSwitch(false); @@ -250,7 +249,7 @@ public boolean getSensorOverride() { public boolean getRejectOverride() { return rejectOverride.getBoolean(false); } - + // logging public void initShuffleboard() { if (Robot.isDebugMode()) { @@ -291,7 +290,7 @@ public void initShuffleboard() { shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1); shuffleboardTab.addBoolean("Intake Left Sensor - ", this::intakeLeftSeesNote).withSize(1, 1); shuffleboardTab.addBoolean("Intake Right Sensor - ", this::intakeRightSeesNote).withSize(1, 1); - + setIntakeInSpeedEntry = shuffleboardTab .addPersistent("Intake in speed - ", INTAKE_IN_SPEED)