diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index c61e90b..e299e4d 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lombok.Getter; import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; @@ -39,6 +38,7 @@ import org.team2342.frc.subsystems.vision.VisionIO; import org.team2342.frc.subsystems.vision.VisionIOPhoton; import org.team2342.frc.subsystems.vision.VisionIOSim; +import org.team2342.lib.util.EnhancedXboxController; public class RobotContainer { @Getter private final Drive drive; @@ -46,7 +46,10 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooser; - @Getter private final CommandXboxController driverController = new CommandXboxController(0); + @Getter + private final EnhancedXboxController driverController = + new EnhancedXboxController(0, DriveConstants.CONTROLLER_DEADBAND); + private final Alert driverControllerAlert = new Alert("Driver controller is disconnected!", AlertType.kError); diff --git a/src/main/java/org/team2342/frc/commands/DriveCommands.java b/src/main/java/org/team2342/frc/commands/DriveCommands.java index 2210ec6..bafcc35 100644 --- a/src/main/java/org/team2342/frc/commands/DriveCommands.java +++ b/src/main/java/org/team2342/frc/commands/DriveCommands.java @@ -7,7 +7,6 @@ package org.team2342.frc.commands; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; @@ -44,9 +43,7 @@ public class DriveCommands { private DriveCommands() {} public static Translation2d getLinearVelocityFromJoysticks(double x, double y) { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband(Math.hypot(x, y), DriveConstants.CONTROLLER_DEADBAND); + double linearMagnitude = Math.hypot(x, y); Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); // Square magnitude for more precise control @@ -72,10 +69,7 @@ public static Command joystickDrive( Translation2d linearVelocity = getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - // Apply rotation deadband - double omega = - MathUtil.applyDeadband( - omegaSupplier.getAsDouble(), DriveConstants.CONTROLLER_DEADBAND); + double omega = omegaSupplier.getAsDouble(); // Square rotation value for more precise control omega = Math.copySign(omega * omega, omega); diff --git a/src/main/java/org/team2342/lib/util/EnhancedXboxController.java b/src/main/java/org/team2342/lib/util/EnhancedXboxController.java new file mode 100644 index 0000000..3934488 --- /dev/null +++ b/src/main/java/org/team2342/lib/util/EnhancedXboxController.java @@ -0,0 +1,89 @@ +// Copyright (c) 2025 Team 2342 +// https://github.com/FRCTeamPhoenix +// +// This source code is licensed under the MIT License. +// See the LICENSE file in the root directory of this project. + +package org.team2342.lib.util; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import lombok.Getter; +import lombok.Setter; + +/** Extended {@link CommandXboxController} class */ +public class EnhancedXboxController extends CommandXboxController { + + @Getter @Setter private double deadband; + + /** + * Construct an instance of a controller. + * + * @param port The port index on the Driver Station that the controller is plugged into. + * @param deadband The deadband that will be applied to the controller sticks. + */ + public EnhancedXboxController(int port, double deadband) { + super(port); + } + + /** + * Construct an instance of a controller with no deadband. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public EnhancedXboxController(int port) { + this(port, 0); + } + + /** + * Set the rumble output for the controller. + * + * @param type Which rumble value to set + * @param intensity The normalized value (0 to 1) to set the rumble to + * @return A command to rumble the controller + */ + public Command rumble(RumbleType type, double intensity) { + return Commands.startEnd( + () -> super.setRumble(type, intensity), () -> super.setRumble(type, 0.0)); + } + + /** + * Set the rumble output for the controller with a specified timeout. + * + * @param type Which rumble value to set + * @param intensity The normalized value (0 to 1) to set the rumble to + * @param seconds The length of time to rumble the controller for in seconds + * @return A command to rumble the controller for the specifed time + */ + public Command timedRumble(RumbleType type, double intensity, double seconds) { + return Commands.runEnd(() -> super.setRumble(type, intensity), () -> super.setRumble(type, 0.0)) + .withTimeout(seconds); + } + + /** Get the deadbanded X axis value of left side of the controller. Right is positive. */ + @Override + public double getLeftX() { + return MathUtil.applyDeadband(super.getLeftX(), deadband); + } + + /** Get the deadbanded Y axis value of left side of the controller. Back is positive. */ + @Override + public double getLeftY() { + return MathUtil.applyDeadband(super.getLeftY(), deadband); + } + + /** Get the deadbanded X axis value of right side of the controller. Right is positive. */ + @Override + public double getRightX() { + return MathUtil.applyDeadband(super.getRightX(), deadband); + } + + /** Get the deadbanded Y axis value of right side of the controller. Back is positive. */ + @Override + public double getRightY() { + return MathUtil.applyDeadband(super.getRightY(), deadband); + } +}