diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d787a30..c5bcee9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,8 +15,9 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -107,6 +108,8 @@ public class RobotContainer { private final SwerveTelemetry swerveTelemetry = new SwerveTelemetry(); + private final PowerDistribution powerDistribution = new PowerDistribution(); + private Debouncer kickerLaserDebouncer = new Debouncer(2.0); Trigger kickerLaserTrigger = new Trigger(() -> spindexer.kickerLaserBroken()); @@ -250,6 +253,8 @@ public RobotContainer() { configureFuelSim(); } + SmartDashboard.putData("Power Distribution", powerDistribution); + SmartDashboard.putData( "Set Arm Down", intake.runOnce(() -> intake.setArmMaxPosition()).ignoringDisable(true)); SmartDashboard.putData( @@ -285,8 +290,14 @@ public RobotContainer() { preShiftShoot.onTrue(Commands.runOnce(() -> canPreShoot = true)); activeHubTrigger.onFalse(Commands.runOnce(() -> canPreShoot = false)); - shiftEndingTrigger.onTrue(driverController.rumbleFor(RumbleType.kBothRumble, 1.0, 3).onlyIf(()-> DriverStation.isTeleop())); - shiftEndingTrigger.onTrue(operatorController.rumbleFor(RumbleType.kBothRumble, 1.0, 3).onlyIf(()-> DriverStation.isTeleop())); + shiftEndingTrigger.onTrue( + driverController + .rumbleFor(RumbleType.kBothRumble, 1.0, 3) + .onlyIf(() -> DriverStation.isTeleop())); + shiftEndingTrigger.onTrue( + operatorController + .rumbleFor(RumbleType.kBothRumble, 1.0, 3) + .onlyIf(() -> DriverStation.isTeleop())); } private void configureFuelSim() {