diff --git a/src/main/java/competition/command_groups/RunCollectorHopperFeederCommandGroup.java b/src/main/java/competition/command_groups/RunCollectorHopperFeederCommandGroup.java index 5740c8de..1fea42fc 100644 --- a/src/main/java/competition/command_groups/RunCollectorHopperFeederCommandGroup.java +++ b/src/main/java/competition/command_groups/RunCollectorHopperFeederCommandGroup.java @@ -14,7 +14,7 @@ public RunCollectorHopperFeederCommandGroup(HopperRollerSubsystem hopper, CollectorIntakeCommand collectorIntakeCommand, ShooterFeederFire shooterFeederFireCommand ) { - var hopperIntakeCommand = hopper.getIntakeCommand(); + var hopperIntakeCommand = hopper.getIntakePulseCommand(); this.addCommands( hopperIntakeCommand.alongWith(collectorIntakeCommand).alongWith(shooterFeederFireCommand) ); diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 004b4626..63e58dc7 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -161,7 +161,7 @@ public void setupDebugGamepad(OperatorInterface operatorInterface, operatorInterface.setupDebugGamepad.getifAvailable(XXboxController.XboxButton.LeftStick) .whileTrue(hopperRollerSubsystem.getEjectCommand()); operatorInterface.setupDebugGamepad.getifAvailable(XXboxController.XboxButton.RightStick) - .whileTrue(hopperRollerSubsystem.getIntakeCommand()); + .whileTrue(hopperRollerSubsystem.getIntakePulseCommand()); operatorInterface.setupDebugGamepad.getPovIfAvailable(90).whileTrue(collectorEjectCommand); operatorInterface.setupDebugGamepad.getPovIfAvailable(270).whileTrue(shooterFeederFire); } diff --git a/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java b/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java index a991234a..020da603 100644 --- a/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java +++ b/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java @@ -1,7 +1,9 @@ package competition.subsystems.hopper_roller; import competition.electrical_contract.ElectricalContract; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.command.BaseSubsystem; import xbot.common.command.NamedRunCommand; import xbot.common.controls.actuators.XCANMotorController; @@ -26,10 +28,13 @@ public class HopperRollerSubsystem extends BaseSubsystem { final DoubleProperty collectPower; final DoubleProperty intakePower; final AngularVelocityProperty intakeVelocity; + final AngularVelocityProperty intakePulseVelocity; final AngularVelocityProperty collectVelocity; final AngularVelocityProperty ejectVelocity; final BooleanProperty useVelocityControl; final DoubleProperty voltageRampTime; + final DoubleProperty intakePulseDuration; + final DoubleProperty intakePulseHighDuration; @Inject public HopperRollerSubsystem(ElectricalContract electricalContract, @@ -67,8 +72,11 @@ public HopperRollerSubsystem(ElectricalContract electricalContract, useVelocityControl = pf.createPersistentProperty("Use Velocity Control", true); intakeVelocity = pf.createPersistentProperty("Intake Velocity", RPM.of(3000)); + intakePulseVelocity = pf.createPersistentProperty("Intake Pulse Velocity", RPM.of(3200)); collectVelocity = pf.createPersistentProperty("Collect Velocity", RPM.of(3000)); ejectVelocity = pf.createPersistentProperty("Eject Velocity", RPM.of(-3000)); + intakePulseDuration = pf.createPersistentProperty("Intake Pulse Duration Seconds", 0.5); + intakePulseHighDuration = pf.createPersistentProperty("Intake Pulse High Duration Seconds", 0.5); if (hopperRollerMotor != null) { hopperRollerMotor.setClosedLoopRampRates( @@ -78,6 +86,13 @@ public HopperRollerSubsystem(ElectricalContract electricalContract, } } + public void setVelocityTarget(AngularVelocity velocity) { + if (hopperRollerMotor == null) { + return; + } + hopperRollerMotor.setVelocityTarget(velocity); + } + public void setEjectPower() { if (hopperRollerMotor == null) { return; @@ -149,4 +164,18 @@ public Command getStopCommand() { } public Command getCollectCommand() {return new NamedRunCommand(getName() + "-collect", this::setCollectPower, this);} + + public Command getIntakePulseCommand() { + var commandGroup = new SequentialCommandGroup(); + commandGroup.setName(getName() + "-intake-pulse"); + commandGroup.addCommands( + new NamedRunCommand( + getName() + "-intake-pulse-low", + () -> setVelocityTarget(intakeVelocity.get()), this).withTimeout(intakePulseDuration.get())); + commandGroup.addCommands( + new NamedRunCommand( + getName() + "-intake-pulse-high", + () -> setVelocityTarget(intakePulseVelocity.get()), this).withTimeout(intakePulseHighDuration.get())); + return commandGroup.repeatedly(); + } }