diff --git a/src/main/java/frc/robot/commands/IntakeSetPivotCommand.java b/src/main/java/frc/robot/commands/IntakeSetPivotCommand.java new file mode 100644 index 0000000..cb7d5b7 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeSetPivotCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands; + +import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem; + +public class IntakeSetPivotCommand { + private final AlgaeIntakeSubsystem intakeSubsystem; + + public IntakeSetPivotCommand(AlgaeIntakeSubsystem subsystem) { + this.intakeSubsystem = subsystem; + } + + public void initialize() { intakeSubsystem.setPivotToVoltage(); } + + public boolean isFinished() { return true; } +} diff --git a/src/main/java/frc/robot/commands/IntakePickupCommand.java b/src/main/java/frc/robot/commands/IntakeSetRollersCommand.java similarity index 69% rename from src/main/java/frc/robot/commands/IntakePickupCommand.java rename to src/main/java/frc/robot/commands/IntakeSetRollersCommand.java index e776059..0317cab 100644 --- a/src/main/java/frc/robot/commands/IntakePickupCommand.java +++ b/src/main/java/frc/robot/commands/IntakeSetRollersCommand.java @@ -2,16 +2,16 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem; -public class IntakePickupCommand extends Command { +public class IntakeSetRollersCommand extends Command { private final AlgaeIntakeSubsystem intakeSubsystem; - public IntakePickupCommand(AlgaeIntakeSubsystem intakeSubsystem) { + public IntakeSetRollersCommand(AlgaeIntakeSubsystem intakeSubsystem) { this.intakeSubsystem = intakeSubsystem; addRequirements(intakeSubsystem); } public void initialize() { - intakeSubsystem.pickup(); + intakeSubsystem.setRollersToVoltage(); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/AlgaeIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/AlgaeIntakeSubsystem.java index 61ec8ad..5edca1e 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/AlgaeIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/AlgaeIntakeSubsystem.java @@ -1,18 +1,25 @@ package frc.robot.subsystems.AlgaeIntakeSubsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.IntakeConstants; public class AlgaeIntakeSubsystem extends SubsystemBase { private final IntakeIO intake; - + private final IntakeIO.IntakeInputsAutoLogged inputs = new IntakeIO.IntakeInputsAutoLogged(); public AlgaeIntakeSubsystem(IntakeIO intake){ this.intake = intake; } - public void pickup() { - intake.pivotDown(); - intake.pivotUp(); + public void setPivotToVoltage() { + intake.setPivotVoltage(IntakeConstants.pivotVoltsTarget); + } + + public void setRollersToVoltage() { + intake.setRollersVoltage(IntakeConstants.rollerVoltsTarget); } + public void periodic() { + intake.updateInputs(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIO.java b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIO.java index bf970bf..6bb6a68 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIO.java @@ -16,6 +16,8 @@ public class IntakeInputsAutoLogged { public default void setRunning(boolean state) {} public default void pivotDown() {} public default void pivotUp() {} + public default void setPivotVoltage(double volts) {} + public default void setRollersVoltage(double volts) {} public default boolean getIsRunning() { return false; } public default boolean coralInside() { return false; } public default void updateInputs(IntakeInputsAutoLogged inputs) {} diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSim.java index 964243b..8aa495a 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSim.java @@ -61,6 +61,16 @@ public void pivotUp() { pivotWheel.setInputVoltage(IntakeConstants.pivotVoltsTarget); } + @Override + public void setPivotVoltage(double volts) { + pivotWheel.setInputVoltage(volts); + } + + @Override + public void setRollersVoltage(double volts) { + rollers.setInputVoltage(volts); + } + @Override public boolean coralInside() { return intakeSim.getGamePiecesAmount() != 0; diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSparkMax.java index e05ebc8..1417057 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSparkMax.java @@ -40,6 +40,16 @@ public void pivotUp() { setRunning(false); } + @Override + public void setPivotVoltage(double volts) { + pivot.setVoltage(volts); + } + + @Override + public void setRollersVoltage(double volts) { + rollers.setVoltage(volts); + } + @Override public void updateInputs(IntakeInputsAutoLogged inputs) { inputs.isRunning = isRunning;