From f54b44fdb9bfb61fb0cc6884fd7621d4c11cb181 Mon Sep 17 00:00:00 2001
From: Sam Carlberg <sam@slfc.dev>
Date: Fri, 8 Dec 2023 19:25:20 -0500
Subject: [PATCH 1/2] Make gradlew script executable

Required for development on linux and mac
---
 gradlew | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 mode change 100644 => 100755 gradlew

diff --git a/gradlew b/gradlew
old mode 100644
new mode 100755

From 0b0b1de82d329f250e2500d4a44708cf2d95532d Mon Sep 17 00:00:00 2001
From: Sam Carlberg <sam@slfc.dev>
Date: Fri, 8 Dec 2023 19:26:11 -0500
Subject: [PATCH 2/2] Use fluent command factories

Instead of manually instantiation commands
---
 src/main/java/frc/robot/RobotContainer.java   | 18 +++----
 .../frc/robot/subsystems/DriveSubsystem.java  | 47 +++++++++++++++++++
 2 files changed, 54 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 249315c4..86e33d57 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,7 +20,6 @@
 import frc.robot.Constants.OIConstants;
 import frc.robot.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import java.util.List;
@@ -49,13 +48,12 @@ public RobotContainer() {
     m_robotDrive.setDefaultCommand(
         // The left stick controls translation of the robot.
         // Turning is controlled by the X axis of the right stick.
-        new RunCommand(
-            () -> m_robotDrive.drive(
-                -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband),
-                true, true),
-            m_robotDrive));
+        m_robotDrive.driveCommand(
+            () -> -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband),
+            () -> -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband),
+            () -> -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband),
+            true,
+            true));
   }
 
   /**
@@ -69,9 +67,7 @@ public RobotContainer() {
    */
   private void configureButtonBindings() {
     new JoystickButton(m_driverController, Button.kR1.value)
-        .whileTrue(new RunCommand(
-            () -> m_robotDrive.setX(),
-            m_robotDrive));
+        .whileTrue(m_robotDrive.setXCommand());
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 30a8d7e0..d814c35a 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -14,9 +14,11 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.DriveConstants;
 import frc.utils.SwerveUtils;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.DoubleSupplier;
 
 public class DriveSubsystem extends SubsystemBase {
   // Create MAXSwerveModules
@@ -186,6 +188,43 @@ else if (angleDif > 0.85*Math.PI) {
     m_rearRight.setDesiredState(swerveModuleStates[3]);
   }
 
+  /**
+   * Creates a command that will drive the robot with dynamic inputs for x, y,
+   * and angular speeds.
+   *
+   * <p>Speed suppliers should be specified as lambdas or method references. If
+   * joystick inputs need to have a deadband or input squaring function applied,
+   * do so within the lambda function.</p>
+   *
+   * @param xSpeed provides speeds in the X direction (forward).
+   * @param ySpeed provides speeds in the Y direction (sideways).
+   * @param rot provides angular speeds.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the
+   *                      field.
+   * @param rateLimit     Whether to enable rate limiting for smoother control.
+   * @return the driving command
+   *
+   * @see <a href="https://docs.wpilib.org/en/stable/docs/software/basic-programming/functions-as-data.html">
+   *   https://docs.wpilib.org/en/stable/docs/software/basic-programming/functions-as-data.html
+   * </a>
+   */
+  public Command driveCommand(
+      DoubleSupplier xSpeed,
+      DoubleSupplier ySpeed,
+      DoubleSupplier rot,
+      boolean fieldRelative,
+      boolean rateLimit) {
+    return run(() -> {
+      drive(
+          xSpeed.getAsDouble(),
+          ySpeed.getAsDouble(),
+          rot.getAsDouble(),
+          fieldRelative,
+          rateLimit
+      );
+    });
+  }
+
   /**
    * Sets the wheels into an X formation to prevent movement.
    */
@@ -196,6 +235,14 @@ public void setX() {
     m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
   }
 
+  /**
+   * Creates a command that continually sets the wheels into an X formation to
+   * prevent movement.
+   */
+  public Command setXCommand() {
+    return run(this::setX);
+  }
+
   /**
    * Sets the swerve ModuleStates.
    *