diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bdd7667b..77aad0cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -37,6 +38,7 @@ import frc.robot.subsystems.mailbox.Mailbox; import frc.robot.subsystems.mailbox.MailboxBelts; import frc.robot.subsystems.mailbox.MailboxPneumatics; +import java.util.function.Supplier; @SuppressWarnings("unused") /** Singleton class that contains all the robot's subsystems, commands, and button bindings. */ @@ -74,14 +76,38 @@ public class RobotContainer { /** Singleton instance of {@link UrMom} for the whole robot. */ public static UrMom urMom = new UrMom(); + /* + * ************* + * * SUPPLIERS * + * ************* + */ + private Supplier scaledControllerLeftXAxisSupplier = () -> getScaledControllerLeftXAxis(); + private Supplier scaledControllerLeftYAxisSupplier = () -> getScaledControllerLeftYAxis(); + private Supplier scaledControllerRightXAxisSupplier = + () -> getScaledControllerRightXAxis(); + private Supplier scaledControllerRightYAxisSupplier = + () -> getScaledControllerRightYAxis(); + private Supplier povUpDirectionSupplier = () -> rawXboxController.getPOV() == 0; + private Supplier povRightDirectionSupplier = () -> rawXboxController.getPOV() == 90; + private Supplier povDownDirectionSupplier = () -> rawXboxController.getPOV() == 180; + private Supplier povLeftDirectionSupplier = () -> rawXboxController.getPOV() == 270; + /* * ************ * * COMMANDS * * ************ */ - private DriveRobotOriented driveRobotOriented = new DriveRobotOriented(); - private DriveFieldOriented driveFieldOriented = new DriveFieldOriented(); + private DriveRobotOriented driveRobotOriented = + new DriveRobotOriented( + scaledControllerLeftYAxisSupplier, + scaledControllerLeftXAxisSupplier, + scaledControllerRightXAxisSupplier); + private DriveFieldOriented driveFieldOriented = + new DriveFieldOriented( + scaledControllerLeftXAxisSupplier, + scaledControllerLeftYAxisSupplier, + scaledControllerRightXAxisSupplier); private EnterXMode enterXMode = new EnterXMode(); private DeployPneumatics deployPneumatics = new DeployPneumatics(); private RunBelts runBelts = new RunBelts(); @@ -121,6 +147,8 @@ public class RobotContainer { public static CommandXboxController driverController = new CommandXboxController(Constants.Controllers.DRIVER_CONTROLLER_PORT); + private static XboxController rawXboxController = driverController.getHID(); + /** Sendable Chooser for autos. */ private SendableChooser autoChooser; diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java index 7caa15e2..ea4b2e3f 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java @@ -16,13 +16,25 @@ import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; +import java.util.function.Supplier; /** Drives the robot in field-oriented mode. */ public class DriveFieldOriented extends Command { private final Drive drive; + private final Supplier xSupplier, ySupplier, zSupplier; - /** Creates a new DriveFieldOriented. */ - public DriveFieldOriented() { + /** + * Drives the robot field-oriented + * + * @param xSupplier The joystick value for the Y axis. [-1, 1] left positive. + * @param ySupplier The joystick value for the Y axis. [-1, 1] back positive. + * @param zSupplier The joystick value for the Z axis. [-1, 1] counterclockwise positive. + */ + public DriveFieldOriented( + Supplier xSupplier, Supplier ySupplier, Supplier zSupplier) { + this.xSupplier = xSupplier; + this.ySupplier = ySupplier; + this.zSupplier = zSupplier; this.drive = RobotContainer.drive; addRequirements(drive); } @@ -34,9 +46,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED; - double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED; - double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED; + double x = this.xSupplier.get() * Constants.Drive.MAX_SPEED; + double y = this.ySupplier.get() * Constants.Drive.MAX_SPEED; + double z = this.zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED; Translation2d translation = new Translation2d(x, y); drive.driveFieldOriented(translation, z); diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java index 5c8a41a4..25589a76 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java @@ -18,6 +18,7 @@ import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; +import java.util.function.Supplier; import swervelib.SwerveController; /** @@ -27,11 +28,39 @@ public class DriveFieldOrientedHeadingSnapping extends Command { private Drive drive; private XboxController controller; + private final Supplier xSupplier, ySupplier, zSupplier; + private final Supplier upSupplier, downSupplier, leftSupplier, rightSupplier; - /** Creates a new DriveFieldOrientedHeadingSnapping. */ - public DriveFieldOrientedHeadingSnapping() { + /** + * Drives the robot field oriented with heading snapping. All values are suppliers to make the + * command more versatile. + * + * @param xSupplier The joystick value for the Y axis. [-1, 1] left positive. + * @param ySupplier The joystick value for the Y axis. [-1, 1] back positive. + * @param zSupplier The joystick value for the Z axis. [-1, 1] counterclockwise positive. + * @param upSupplier If true, the robot heading will be set to move towards the opposing alliance + * wall + * @param downSupplier If true, the robot heading will be set to move towards our alliance wall + * @param leftSupplier If true, the robot heading will be set to move towards the left + * @param rightSupplier If true, the robot heading will be set to move towards the right + */ + public DriveFieldOrientedHeadingSnapping( + Supplier xSupplier, + Supplier ySupplier, + Supplier zSupplier, + Supplier upSupplier, + Supplier downSupplier, + Supplier leftSupplier, + Supplier rightSupplier) { this.controller = RobotContainer.driverController.getHID(); this.drive = RobotContainer.drive; + this.xSupplier = xSupplier; + this.ySupplier = ySupplier; + this.zSupplier = zSupplier; + this.upSupplier = upSupplier; + this.downSupplier = downSupplier; + this.leftSupplier = leftSupplier; + this.rightSupplier = rightSupplier; addRequirements(RobotContainer.drive); } @@ -46,33 +75,25 @@ public void initialize() { public void execute() { double headingX = 0; double headingY = 0; - if (controller.getPOV() == 0) { + if (upSupplier.get()) { headingY = -1; } - if (controller.getPOV() == 90) { + if (rightSupplier.get()) { headingX = 1; } - if (controller.getPOV() == 180) { + if (downSupplier.get()) { headingY = 1; } - if (controller.getPOV() == 270) { + if (leftSupplier.get()) { headingX = -1; } ChassisSpeeds desiredSpeeds = - drive.getTargetSpeeds( - RobotContainer.getScaledControllerLeftXAxis(), - RobotContainer.getScaledControllerLeftYAxis(), - headingX, - headingY); + drive.getTargetSpeeds(xSupplier.get(), ySupplier.get(), headingX, headingY); Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - if (headingX == 0 - && headingY == 0 - && Math.abs(RobotContainer.getScaledControllerRightXAxis()) > 0) { - drive.driveFieldOriented( - translation, - (RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED)); + if (headingX == 0 && headingY == 0 && Math.abs(zSupplier.get()) > 0) { + drive.driveFieldOriented(translation, zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED); } else { drive.driveFieldOriented(translation, desiredSpeeds.omegaRadiansPerSecond); } diff --git a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java index 168bc9ba..41e141fc 100644 --- a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java +++ b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java @@ -16,14 +16,26 @@ import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; +import java.util.function.Supplier; /** Drives the robot in robot-oriented mode. Default command for {@link Drive} subsystem. */ public class DriveRobotOriented extends Command { private final Drive drive; + private final Supplier xSupplier, ySupplier, zSupplier; - /** Creates a new DriveRobotOriented. */ - public DriveRobotOriented() { + /** + * Drives the robot robot-oriented + * + * @param xSupplier The joystick value for the Y axis. [-1, 1] left positive. + * @param ySupplier The joystick value for the Y axis. [-1, 1] back positive. + * @param zSupplier The joystick value for the Z axis. [-1, 1] counterclockwise positive. + */ + public DriveRobotOriented( + Supplier xSupplier, Supplier ySupplier, Supplier zSupplier) { this.drive = RobotContainer.drive; + this.xSupplier = xSupplier; + this.ySupplier = ySupplier; + this.zSupplier = zSupplier; addRequirements(drive); } @@ -34,9 +46,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED; - double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED; - double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED; + double x = xSupplier.get() * Constants.Drive.MAX_SPEED; + double y = ySupplier.get() * Constants.Drive.MAX_SPEED; + double z = zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED; Translation2d translation = new Translation2d(x, y); drive.driveRobotOriented(translation, z);