diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 73c12cf..2690bec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -75,7 +75,10 @@ public Robot() { // Configure brownout voltage RobotController.setBrownoutVoltage(6.0); - // Create RobotConatiner + // Disable joysick warnings in Driver Station + DriverStation.silenceJoystickConnectionWarning(true); + + // Create RobotContainer robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9defebc..1a3834d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,10 +23,15 @@ import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSpark; import frc.robot.util.AllianceFlipUtil; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class RobotContainer { + // Button Binding variables + @AutoLogOutput private boolean robotRelativeEnabled = false; + @AutoLogOutput private boolean slowModeEnabled = false; + // Load PoseEstimator class private final PoseEstimator poseEstimator = PoseEstimator.getInstance(); @@ -47,8 +52,6 @@ public class RobotContainer { private final LoggedNetworkNumber endgameAlert2 = new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0); - private boolean slowModeEnabled; - public RobotContainer() { switch (Constants.getMode()) { case REAL -> { @@ -145,6 +148,16 @@ private void configureButtonBindings() { // Make slow mode toggleable controller.y().toggleOnTrue(Commands.runOnce(() -> slowModeEnabled = !slowModeEnabled)); + controller + .leftBumper() + .and(controller.rightBumper()) + .onTrue(Commands.runOnce(() -> robotRelativeEnabled = true)); + + controller + .leftBumper() + .and(controller.rightBumper()) + .onFalse(Commands.runOnce(() -> robotRelativeEnabled = false)); + // Default command, normal field-relative drive driveBase.setDefaultCommand( DriveCommands.joystickDrive( @@ -153,31 +166,52 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> -controller.getRightX(), () -> slowModeEnabled, - () -> controller.leftBumper().and(controller.rightBumper()).getAsBoolean())); + () -> robotRelativeEnabled)); // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); // L1 controller .povLeft() - .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); + .onTrue( + Commands.runOnce( + () -> { + robotRelativeEnabled = true; + slowModeEnabled = true; + }) + .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL)))); + // L2 controller .povUp() .onTrue( - Commands.either( - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)), - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), - controller.b().negate().debounce(0.25))); + Commands.runOnce( + () -> { + robotRelativeEnabled = true; + slowModeEnabled = true; + }) + .andThen( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)), + Commands.runOnce( + () -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25)))); // L3 controller .povRight() .onTrue( - Commands.either( - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)), - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), - controller.b().negate().debounce(0.25))); + Commands.runOnce( + () -> { + robotRelativeEnabled = true; + slowModeEnabled = true; + }) + .andThen( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)), + Commands.runOnce( + () -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25)))); // Intake controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); @@ -185,12 +219,19 @@ private void configureButtonBindings() { controller .rightTrigger() .onTrue( - Commands.either( - dispenserBase - .eject(elevatorBase::getGoal) - .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))), - IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), - controller.leftTrigger().negate().debounce(0.25))); + Commands.runOnce( + () -> { + robotRelativeEnabled = false; + slowModeEnabled = false; + }) + .andThen( + Commands.either( + dispenserBase + .eject(elevatorBase::getGoal) + .andThen( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))), + IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), + controller.leftTrigger().negate().debounce(0.25)))); // Home Elevator controller diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 269db0f..823fa88 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -24,6 +24,7 @@ class Module { private static final LoggedTunableNumber driveIZone = new LoggedTunableNumber("Drive/Module/DrivekIZone"); private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); + private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI"); private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); static { @@ -46,6 +47,7 @@ class Module { drivekD.initDefault(0.0); driveIZone.initDefault(0.0); turnkP.initDefault(10.0); + turnkI.initDefault(0.0); turnkD.initDefault(0.0); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index a29a717..3dfd80e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -169,9 +169,14 @@ public void runTurnOpenLoop(double output) { @Override public void runDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveFeedforward.calculate(velocityRadPerSec); + double ffVolts = + driveFeedforward.calculate( + velocityRadPerSec + // * 1 / (Math.PI * 2) + ); driveController.setReference( velocityRadPerSec, + // * 1 / (Math.PI * 2), ControlType.kVelocity, ClosedLoopSlot.kSlot0, ffVolts, @@ -182,7 +187,11 @@ public void runDriveVelocity(double velocityRadPerSec) { public void runTurnPosition(Rotation2d rotation) { // Because internal encoder is relative, we want to wrap to the range -π to π radians. var updatedSetpoint = MathUtil.angleModulus(rotation.getRadians()); - turnController.setReference(updatedSetpoint, ControlType.kPosition); + turnController.setReference( + updatedSetpoint + // * 1 / (Math.PI * 2) + , + ControlType.kPosition); } @Override diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java index 4e1e513..615c3f4 100644 --- a/src/main/java/frc/robot/util/AlertsUtil.java +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -37,6 +37,9 @@ public static AlertsUtil getInstance() { private final Alert lowBatteryVoltageAlert = new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); + private final Alert joystickDisconnectedAlert = + new Alert("At least one joystick button is not detected", AlertType.kWarning); + private final Debouncer joystickDebouncer = new Debouncer(2); // Program Alerts private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); @@ -75,6 +78,9 @@ public void periodic() { batteryVoltageDebouncer.calculate( RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + joystickDisconnectedAlert.set( + joystickDebouncer.calculate(DriverStation.isJoystickConnected(0))); + // Update Program Alerts // TODO