REFACTOR: subsystem shouldn't have individual command factories

Author: dabeycorn

src/main/java/wmironpatriots/Robot.java

@@ -7,6 +7,7 @@
 package wmironpatriots;
 
 import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -15,21 +16,19 @@
 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.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import lib.drivers.LoggedCommandRobot;
 import monologue.Monologue;
-import wmironpatriots.commands.factories.SwerveCommandFactory;
 import wmironpatriots.subsystems.Swerve.Swerve;
 
 public class Robot extends LoggedCommandRobot {
-  private final CommandXboxController driver = new CommandXboxController(0);
+  private final CommandPS5Controller driver = new CommandPS5Controller(0);
   private final CommandXboxController operator = new CommandXboxController(1);
 
   private final Swerve swerve = Swerve.create();
 
-  private final SwerveCommandFactory swerveCmdFactory = new SwerveCommandFactory(swerve);
-
   private final Alert browningOut;
 
   public Robot() {
@@ -75,14 +74,25 @@ public Robot() {
   public void configureBindings() {}
 
   public void configureGameBehavior() {
-    swerve.setDefaultCommand(swerveCmdFactory.defaultCommand());
-
-    inTeleoperated.whileTrue(
-        swerveCmdFactory
-            .teleopDrive(driver::getLeftX, driver::getLeftY, driver::getRightX)
+    swerve.setDefaultCommand(
+        swerve
+            .driveFromMagnitudes(
+                () -> modifyJoystick(driver.getLeftY() * -1),
+                () -> modifyJoystick(driver.getLeftX() * -1),
+                () -> modifyJoystick(driver.getRightX() * -1))
             .repeatedly());
   }
 
+  /**
+   * Squares and deadbands a joystick value
+   *
+   * @param val joystick value
+   * @return modified joystick value
+   */
+  public static double modifyJoystick(double val) {
+    return MathUtil.applyDeadband(Math.abs(Math.pow(val, 2)) * Math.signum(val), 0.08);
+  }
+
   /** Command for driver controller rumble */
   private Command rumbleDriver(double value) {
     return Commands.run(() -> driver.setRumble(RumbleType.kBothRumble, value));

src/main/java/wmironpatriots/commands/factories/SwerveCommandFactory.java

@@ -6,6 +6,8 @@
 
 package wmironpatriots.subsystems.Swerve;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -22,6 +24,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.function.DoubleSupplier;
 import wmironpatriots.Constants;
 import wmironpatriots.Robot;
 import wmironpatriots.subsystems.Swerve.gyro.GyroHardware;
@@ -43,7 +46,6 @@ public static Swerve create() {
 
       return new Swerve(new GyroHardwareComp(), modules);
     } else {
-      // ! PLACEHOLDER FOR SIM HARDWARE INIT
       for (int i = 0; i < modules.length; i++) {
         modules[i] = new Module(new ModuleHardwareSim(moduleConfigs[i]));
       }
@@ -92,7 +94,7 @@ public void periodic() {
     }
 
     if (DriverStation.isDisabled()) {
-      stop();
+      stopAndLock();
     }
   }
 
@@ -105,6 +107,46 @@ public void simulationPeriodic() {
                 !Double.isNaN(angularRate) ? angularRate * Constants.LOOPTIME.in(Seconds) : 0));
   }
 
+  /**
+   * Drives robot based on magnitudes
+   *
+   * @param xSpeedMagnitude Input stream representing desired x speed magnitude
+   * @param ySpeedMagnitude Input stream representing desired y speed magnitude
+   * @param angularRateMagnitude Input stream representing desiredd angular rate magnitude
+   * @return {@link Command}
+   */
+  public Command driveFromMagnitudes(
+      DoubleSupplier xSpeedMagnitude,
+      DoubleSupplier ySpeedMagnitude,
+      DoubleSupplier angularRateMagnitude) {
+    return this.run(
+        () ->
+            setChassisSpeeds(
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                    xSpeedMagnitude.getAsDouble()
+                        * SwerveConstants.MAX_LINEAR_SPEED.in(MetersPerSecond),
+                    ySpeedMagnitude.getAsDouble()
+                        * SwerveConstants.MAX_LINEAR_SPEED.in(MetersPerSecond),
+                    angularRateMagnitude.getAsDouble()
+                        * SwerveConstants.MAX_ANGULAR_RATE.in(RadiansPerSecond),
+                    getHeadingRotation2d())));
+  }
+
+  public Command stopAndLock() {
+    return this.runOnce(
+        () -> {
+          var states =
+              new SwerveModuleState[] {
+                new SwerveModuleState(0.0, Rotation2d.fromDegrees(45)),
+                new SwerveModuleState(0.0, Rotation2d.fromDegrees(-45)),
+                new SwerveModuleState(0.0, Rotation2d.fromDegrees(45)),
+                new SwerveModuleState(0.0, Rotation2d.fromDegrees(-45)),
+              };
+
+          setSwerveModuleStates(states);
+        });
+  }
+
   /**
    * Converts a {@link ChassisSpeeds} object representing robot relative speeds into {@link
    * SwerveModuleState} setpoint for each module
@@ -122,9 +164,19 @@ public void setChassisSpeeds(ChassisSpeeds speeds) {
     speeds = ChassisSpeeds.discretize(speeds, Constants.LOOPTIME.in(Seconds));
 
     var states = kinematics.toSwerveModuleStates(speeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_LINEAR_SPEED);
-    setpoint.set(states);
+    SwerveDriveKinematics.desaturateWheelSpeeds(
+        states, SwerveConstants.MAX_LINEAR_SPEED.in(MetersPerSecond));
+
+    setSwerveModuleStates(states);
+  }
 
+  /**
+   * Sets the setpoints of each swerve module
+   *
+   * @param states {@link SwerveModuleState} array of desired states
+   */
+  public void setSwerveModuleStates(SwerveModuleState[] states) {
+    setpoint.set(states);
     for (int i = 0; i < modules.length; i++) {
       modules[i].setSetpoints(states[i]);
     }

src/main/java/wmironpatriots/subsystems/Swerve/Swerve.java

@@ -70,10 +70,10 @@ public static record ModuleConfig(
 
   public static final Translation2d[] MODULE_OFFSETS =
       new Translation2d[] {
-        new Translation2d(TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0),
         new Translation2d(TRACK_WIDTH.in(Meters) / 2.0, TRACK_WIDTH.in(Meters) / 2.0),
-        new Translation2d(-TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0),
-        new Translation2d(TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0)
+        new Translation2d(TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0),
+        new Translation2d(-TRACK_WIDTH.in(Meters) / 2.0, TRACK_WIDTH.in(Meters) / 2.0),
+        new Translation2d(-TRACK_WIDTH.in(Meters) / 2.0, -TRACK_WIDTH.in(Meters) / 2.0)
       };
 
   public static final ModuleConfig[] MODULE_CONFIGS =