Merge pull request #19 from Team334/operatorLogic

changed logic for passoff and added some other controls
Team334 · Jan 20, 2025 · de05384 · de05384
2 parents 574e7a5 + 4698338
commit de05384
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 2 deletions.
diff --git 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
@@ -6,6 +6,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 frc.robot.Constants;
@@ -54,6 +55,8 @@ public Distance getHeight() {
 
   private final Consumer<WristevatorSetpoint> _wristevatorSetpointSetter;
 
+  private DigitalInput _elevatorSwitch;
+
   public Wristevator(Consumer<WristevatorSetpoint> wristevatorSetpointSetter) {
     _wristevatorSetpointSetter = wristevatorSetpointSetter;
   }
@@ -68,6 +71,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(() -> {})