diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a2a7c00..91891b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,10 +23,13 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.FaultLogger; import frc.lib.InputStream; import frc.robot.Constants.Ports; @@ -126,6 +129,8 @@ public Robot(NetworkTableInstance ntInst) { configureDriverBindings(); configureOperatorBindings(); + new Trigger(_serializer::getBackBeam).onTrue(rumbleControllers(1, 1)); + SmartDashboard.putData( "Robot Self Check", sequence( @@ -210,15 +215,27 @@ private void configureOperatorBindings() { _operatorController .rightBumper() - .and(() -> getSetpoint() == HOME) + .and(() -> _wristevator.atHome()) .whileTrue(Superstructure.passoff(_intake, _serializer, _manipulator)); _operatorController .rightBumper() - .and(() -> getSetpoint() != HOME) + .and(() -> !_wristevator.atHome()) .whileTrue(Superstructure.groundIntake(_intake, _serializer)); _operatorController.leftBumper().whileTrue(Superstructure.groundOuttake(_intake, _serializer)); + + _operatorController.rightTrigger().whileTrue(_manipulator.setSpeed(+0)); + _operatorController.leftTrigger().whileTrue(_manipulator.setSpeed(-0)); + } + + /** Rumble the driver and operator controllers for some amount of seconds. */ + private Command rumbleControllers(double rumble, double seconds) { + return run(() -> { + _driverController.getHID().setRumble(RumbleType.kBothRumble, rumble); + _operatorController.getHID().setRumble(RumbleType.kBothRumble, rumble); + }) + .withTimeout(seconds); } /** diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java index 360e2e0..9e95dc6 100644 --- a/src/main/java/frc/robot/subsystems/Wristevator.java +++ b/src/main/java/frc/robot/subsystems/Wristevator.java @@ -5,6 +5,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import frc.lib.AdvancedSubsystem; import java.util.function.Consumer; @@ -44,6 +45,8 @@ public Distance getHeight() { private final Consumer _wristevatorSetpointSetter; + private DigitalInput _elevatorSwitch; + public Wristevator(Consumer wristevatorSetpointSetter) { _wristevatorSetpointSetter = wristevatorSetpointSetter; } @@ -58,6 +61,11 @@ public double getAngle() { return 0; } + @Logged(name = "Is at Home") + public boolean atHome() { + return _elevatorSwitch.get(); + } + /** Set the wristevator to a setpoint. */ public Command setSetpoint(WristevatorSetpoint setpoint) { return run(() -> {})