From c32189ecc2bb93f24768cda2f38f80b72d05e756 Mon Sep 17 00:00:00 2001 From: Elvis Osmanov Date: Sun, 19 Jan 2025 20:14:28 -0500 Subject: [PATCH 1/2] changed logic for passoff and added someother controls --- src/main/java/frc/robot/Robot.java | 18 ++++++++++++++++-- .../java/frc/robot/subsystems/Wristevator.java | 8 ++++++++ 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a2a7c00..96da0f7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,10 +23,12 @@ 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.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; @@ -183,6 +185,10 @@ private void configureDefaultCommands() { InputStream.of(_operatorController::getLeftY) .negate() .scale(WristevatorConstants.maxWristSpeed.in(RadiansPerSecond)))); + + new Trigger(_serializer::getBackBeam) + .onTrue( + run(() -> rumbleControllers(1)).withTimeout(2).finallyDo(() -> rumbleControllers(0))); } private void configureDriverBindings() { @@ -210,15 +216,23 @@ 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)); + } + + private void rumbleControllers(double rumble) { + _driverController.getHID().setRumble(RumbleType.kBothRumble, rumble); + _operatorController.getHID().setRumble(RumbleType.kBothRumble, rumble); } /** 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(() -> {}) From 46983383d242fbc046ce40acc05e862cbe1397bd Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Mon, 20 Jan 2025 09:59:02 -0500 Subject: [PATCH 2/2] made rumble controllers into a command --- src/main/java/frc/robot/Robot.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 96da0f7..91891b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,6 +26,7 @@ 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; @@ -128,6 +129,8 @@ public Robot(NetworkTableInstance ntInst) { configureDriverBindings(); configureOperatorBindings(); + new Trigger(_serializer::getBackBeam).onTrue(rumbleControllers(1, 1)); + SmartDashboard.putData( "Robot Self Check", sequence( @@ -185,10 +188,6 @@ private void configureDefaultCommands() { InputStream.of(_operatorController::getLeftY) .negate() .scale(WristevatorConstants.maxWristSpeed.in(RadiansPerSecond)))); - - new Trigger(_serializer::getBackBeam) - .onTrue( - run(() -> rumbleControllers(1)).withTimeout(2).finallyDo(() -> rumbleControllers(0))); } private void configureDriverBindings() { @@ -230,9 +229,13 @@ private void configureOperatorBindings() { _operatorController.leftTrigger().whileTrue(_manipulator.setSpeed(-0)); } - private void rumbleControllers(double rumble) { - _driverController.getHID().setRumble(RumbleType.kBothRumble, rumble); - _operatorController.getHID().setRumble(RumbleType.kBothRumble, rumble); + /** 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); } /**