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..811040d --- /dev/null +++ b/src/main/java/frc/robot/Commands/AdvancedIntakeSequence.java @@ -0,0 +1,30 @@ +// 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 bd18856..fd08b82 100644 --- a/src/main/java/frc/robot/Commands/IntakeCommand.java +++ b/src/main/java/frc/robot/Commands/IntakeCommand.java @@ -37,7 +37,7 @@ public void initialize() { intakeSubsystem.SetMotorPower(Constants.Intake.CUBE_UNLOADING_SPEED); } - //set the object type - used in the stoter currents + // set the object type - used in the stoter currents intakeSubsystem.SetObjectType(isCone); } @@ -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 c0b34fb..0059372 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 d66a071..247e298 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,6 +27,8 @@ 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; } public final class Wrist { public static final int DRIVER_CONTROLLER_PORT = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6e3938..5276836 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.SequentialCommands; import frc.robot.subsystems.WristSubsystem; +import frc.robot.Commands.AdvancedIntakeSequence; +import frc.robot.Subsystems.IntakeSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -76,12 +78,9 @@ 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)); - - // stop the motors - driverB.x().onTrue(new StopIntakeMotorCommand(intakeSubsystem)); + 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)); } } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index c88d067..0382b70 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -53,8 +53,8 @@ public void SetMotorPower(double motorPower) { this.motorPower = motorPower; } - //to set the object as either a cone or a cube - //to be called in commands alongside of set motor power + // to set the object as either a cone or a cube + // to be called in commands alongside of set motor power public void SetObjectType(boolean isCone) { this.isCone = isCone; } @@ -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); } }