diff --git a/build.gradle b/build.gradle index a4506c2..6ca535d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2023.4.3" id "com.diffplug.spotless" version "6.13.0" id "me.champeau.jmh" version "0.6.8" } diff --git a/src/main/java/frc/robot/Commands/AdvancedIntakeSequence.java b/src/main/java/frc/robot/Commands/AdvancedIntakeSequence.java new file mode 100644 index 0000000..e2c45f5 --- /dev/null +++ b/src/main/java/frc/robot/Commands/AdvancedIntakeSequence.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.Subsystems.IntakeSubsystem; + +// 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 AdvancedIntakeSequence extends SequentialCommandGroup { + /** Creates a new AdvancedIntakeSequence. */ + public AdvancedIntakeSequence(IntakeSubsystem intakeSubsystem, boolean isCone, boolean isIntake) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + if(isIntake){ + addCommands( new IntakeCommand(intakeSubsystem, isCone, isIntake), new StopIntakeMotorCommand(intakeSubsystem)); + }else{ + addCommands( new IntakeCommand(intakeSubsystem, isCone, isIntake).withTimeout(Constants.Intake.UNLOADING_WAIT_TIME), new StopIntakeMotorCommand(intakeSubsystem) ); + } + } +} diff --git a/src/main/java/frc/robot/Commands/IntakeCommand.java b/src/main/java/frc/robot/Commands/IntakeCommand.java index 96e7410..491721f 100644 --- a/src/main/java/frc/robot/Commands/IntakeCommand.java +++ b/src/main/java/frc/robot/Commands/IntakeCommand.java @@ -52,6 +52,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return intakeSubsystem.IsLoaded(); } } diff --git a/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java b/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java index 5c5b61b..fe4b66e 100644 --- a/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java +++ b/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java @@ -36,6 +36,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 806409a..d5261fd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,5 +27,7 @@ public static class Intake { public static final double CUBE_STATOR_LIMIT = 90; public static final double CONE_STATOR_LIMIT = 70; + + public static final double UNLOADING_WAIT_TIME = 2f; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c09eaf6..1af7d2d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Commands.AdvancedIntakeSequence; import frc.robot.Commands.IntakeCommand; import frc.robot.Commands.StopIntakeMotorCommand; import frc.robot.Subsystems.IntakeSubsystem; @@ -63,12 +64,10 @@ public void containerMatchStarting() { */ private void configureButtonBindings() { // intake and outtake - driverB.rightTrigger().onTrue(new IntakeCommand(intakeSubsystem, false, true)); - driverB.leftTrigger().onTrue(new IntakeCommand(intakeSubsystem, true, true)); - driverB.rightBumper().onTrue(new IntakeCommand(intakeSubsystem, false, false)); - driverB.leftBumper().onTrue(new IntakeCommand(intakeSubsystem, true, false)); + driverB.rightTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, true)); + driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true)); + driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false)); + driverB.leftBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, false)); - // stop the motors - driverB.x().onTrue(new StopIntakeMotorCommand(intakeSubsystem)); } } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index f5077cf..67a5097 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -67,7 +67,7 @@ public void periodic() { // set the motor power to the var intakeMotor.set(TalonFXControlMode.PercentOutput, motorPower); - // if we are done loading than set the motor power to 0 - if (IsLoaded()) SetMotorPower(0); + // // if we are done loading than set the motor power to 0 + // if (IsLoaded()) SetMotorPower(0); } }