Merge branch 'robot-logger' into swerve

src/main/java/frc4388/robot/Constants.java

@@ -71,11 +71,13 @@ public final class Constants {
     // swerve auto constants
     public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
     public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
-    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
-            15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
-    
-    public static final double MAX_VEL = 5.0;
-    public static final double MAX_ACC = 5.0;
+    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController (15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+    public static final boolean PATH_RECORD_VELOCITY = true;
+    public static final double PATH_MAX_VELOCITY = 5.0;
+    public static final double PATH_MAX_ACCELERATION = 5.0;
+    public static final double MIN_WAYPOINT_ANGLE = 20;
+    public static final double MIN_WAYPOINT_DISTANCE = 0.1;
+    public static final double MIN_WAYPOINT_VELOCITY = 0.1;
 
     // swerve configuration
     public static final double NEUTRAL_DEADBAND = 0.04;
@@ -127,7 +129,7 @@ public final class Constants {
     /* PID Constants Shooter */
     public static final int CLOSED_LOOP_TIME_MS = 1;
 
-   public static final int SHOOTER_TIMEOUT_MS = 32;
+    public static final int SHOOTER_TIMEOUT_MS = 32;
     public static final int SHOOTER_SLOT_IDX = 0;
     public static final int SHOOTER_PID_LOOP_IDX = 1;
     public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
@@ -173,11 +175,11 @@ public final class Constants {
 
   }
   public static final class VisionConstants {
-      public static final double TURN_P_VALUE = 0.8;
-      public static final double X_ANGLE_ERROR = 0.5;
-      public static final double GRAV = 385.83;
-      public static final double TARGET_HEIGHT = 67.5;
-      public static final double FOV = 29.8; //Field of view limelight
-      public static final double LIME_ANGLE = 24.7;
-    }
+    public static final double TURN_P_VALUE = 0.8;
+    public static final double X_ANGLE_ERROR = 0.5;
+    public static final double GRAV = 385.83;
+    public static final double TARGET_HEIGHT = 67.5;
+    public static final double FOV = 29.8; //Field of view limelight
+    public static final double LIME_ANGLE = 24.7;
+  }
 }

src/main/java/frc4388/robot/Main.java

@@ -5,6 +5,12 @@
 package frc4388.robot;
 
 
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+import com.diffplug.common.base.DurianPlugins;
+import com.diffplug.common.base.Errors;
+
 import edu.wpi.first.wpilibj.RobotBase;
 import frc4388.utility.AnsiLogging;
 
@@ -24,6 +30,7 @@ public final class Main {
    */
   public static void main(String... args) {
     AnsiLogging.systemInstall();
+    DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage));
     RobotBase.startRobot(Robot::new);
   }
 }

src/main/java/frc4388/robot/Robot.java

@@ -4,14 +4,22 @@
 
 package frc4388.robot;
 
+import java.io.File;
+import java.nio.file.Files;
+import java.nio.file.Path;
 import java.util.logging.Level;
 import java.util.logging.Logger;
+import java.util.stream.IntStream;
+
+import com.diffplug.common.base.Errors;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.utility.RobotTime;
 
 /**
@@ -22,7 +30,7 @@ import frc4388.utility.RobotTime;
  * project.
  */
 public class Robot extends TimedRobot {
-  private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
+  private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
   Command m_autonomousCommand;
   
   private RobotTime m_robotTime = RobotTime.getInstance();
@@ -34,20 +42,51 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
-    if (org.fusesource.jansi.Ansi.isEnabled()) {
-      LOGGER.log(Level.ALL, "Logging Test 1/8");
-      LOGGER.log(Level.SEVERE, "Logging Test 2/8");
-      LOGGER.log(Level.WARNING, "Logging Test 3/8");
-      LOGGER.log(Level.INFO, "Logging Test 4/8");
-      LOGGER.log(Level.CONFIG, "Logging Test 5/8");
-      LOGGER.log(Level.FINE, "Logging Test 6/8");
-      LOGGER.log(Level.FINER, "Logging Test 7/8");
-      LOGGER.log(Level.FINEST, "Logging Test 8/8");
-    }
+    LOGGER.log(Level.ALL, "Logging Test 1/8");
+    LOGGER.log(Level.SEVERE, "Logging Test 2/8");
+    LOGGER.log(Level.WARNING, "Logging Test 3/8");
+    LOGGER.log(Level.INFO, "Logging Test 4/8");
+    LOGGER.log(Level.CONFIG, "Logging Test 5/8");
+    LOGGER.log(Level.FINE, "Logging Test 6/8");
+    LOGGER.log(Level.FINER, "Logging Test 7/8");
+    LOGGER.log(Level.FINEST, "Logging Test 8/8");
+    Errors.log().run(() -> { throw new Throwable("Exception Test"); });
+
+    // var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile());
+    // LOGGER.finest(path::toString);
     LOGGER.fine("robotInit()");
+    // LOGGER.fine("Sssssssssh.");
+    // DriverStation.silenceJoystickConnectionWarning(true);
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
+    SmartDashboard.putData(CommandScheduler.getInstance());
+    SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          Runtime runtime = Runtime.getRuntime();
+          long totalMemory = runtime.totalMemory() / 1_000_000;
+          long freeMemory = runtime.freeMemory() / 1_000_000;
+          long maxMemory = runtime.maxMemory() / 1_000_000;
+          return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
+        }
+        return "Not Running";
+      }
+    });
+    SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          File deploy = Filesystem.getDeployDirectory();
+          long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000;
+          long usableSpace = deploy.getUsableSpace() / 1_000_000;
+          return usedSpace + " MB / " + usableSpace + " MB";
+        }
+        return "Not Running";
+      }
+    });
   }
 
   /**
@@ -82,6 +121,15 @@ public class Robot extends TimedRobot {
   public void disabledInit() {
     LOGGER.fine("disabledInit()");
     m_robotTime.endMatchTime();
+    if (isTest()) {
+      // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+      File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+      if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+        m_robotContainer.createPath(null, null, false).write(outputFile);
+        LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
+      } else
+        LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
+    }
   }
 
   @Override
@@ -132,6 +180,11 @@ public class Robot extends TimedRobot {
   public void teleopPeriodic() {
   }
 
+  @Override
+  public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+
   /**
    * This function is called periodically during test mode.
    */

src/main/java/frc4388/robot/RobotContainer.java

@@ -4,36 +4,59 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-
+import java.io.File;
+import java.io.IOException;
+import java.io.StringWriter;
+import java.nio.file.FileSystems;
+import java.nio.file.Path;
+import java.nio.file.StandardWatchEventKinds;
+import java.nio.file.WatchEvent;
+import java.nio.file.WatchKey;
+import java.time.Clock;
+import java.time.ZoneId;
+import java.time.ZonedDateTime;
+import java.time.format.DateTimeFormatter;
 import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
+import java.util.Optional;
+import java.util.function.Function;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+import java.util.stream.Collectors;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.diffplug.common.base.Errors;
 import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
 import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
 import com.pathplanner.lib.commands.PPSwerveControllerCommand;
 
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.NotifierCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
-import frc4388.robot.Constants.*;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
@@ -42,6 +65,9 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.ListeningSendableChooser;
+import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
@@ -52,17 +78,12 @@ import frc4388.utility.controller.DeadbandedXboxController;
  * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
   /* Subsystems */
-  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, 
-                                                                m_robotMap.leftBack, 
-                                                                m_robotMap.rightFront, 
-                                                                m_robotMap.rightBack, 
-                                                                m_robotMap.gyro);
-
-  private final TalonFX m_testMotor = new TalonFX(23);
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
@@ -73,6 +94,18 @@ public class RobotContainer {
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
+  /* Autonomous */
+  private PathPlannerTrajectory loadedPathTrajectory = null;
+  private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
+  private final List<Waypoint> pathPoints = new ArrayList<>();
+  private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
+  private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
+
+  private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH:mm:ss.SSS'.path'");
+  private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
+  private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
+  private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
+
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
@@ -89,15 +122,17 @@ public class RobotContainer {
 
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
-              getDriverController().getLeftX(),
-              getDriverController().getLeftY(),
-              getDriverController().getRightX(),
-              getDriverController().getRightY(),
-          true),
-           m_robotSwerveDrive));
+            getDriverController().getLeftX(),
+            getDriverController().getLeftY(),
+            getDriverController().getRightX(),
+            getDriverController().getRightY(),
+            true),
+            m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
-    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
+    autoInit();
+    recordInit();
   }
 
   /**
@@ -110,21 +145,18 @@ public class RobotContainer {
     /* Driver Buttons */
     // "XboxController.Button.kBack" was undefined yet, 7 works just fine
     new JoystickButton(getDriverController(), 7)
-      .whenPressed(() -> m_robotSwerveDrive.resetGyro());
+      .whenPressed(m_robotSwerveDrive::resetGyro);
 
     new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
-    // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
-      .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
-
+        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
+        .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
 
     new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
-    // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
-      .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
-    
-    new JoystickButton(getDriverController(), XboxController.Button.kA.value)
+        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
+        .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
 
-      .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-      //.whenPressed(this::resetOdometry);
+    new JoystickButton(getDriverController(), XboxController.Button.kA.value)
+        .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
 
     /* Operator Buttons */
     // activates "Lit Mode"
@@ -141,81 +173,33 @@ public class RobotContainer {
         .whenReleased(() -> m_robotHood.runHood(0.d));
   }
 
-  /**
-   * Generate autonomous
-   * @param maxVel max velocity for the path (null to override default value of 5.0)
-   * @param maxAccel max acceleration for the path (null to override default value of 5.0)
-   * @param inputs strings (path names) or commands you want to run (in order)
-   * @return array of commands, which can then be processed in a command group
-   */
-  public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
-
-    // default vel and acc
-    maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
-    maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
-
-    ArrayList<Command> commands = new ArrayList<Command>();
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
-
-    // pids controlling the path
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    // parse input
-    for (int i=0; i<inputs.length; i++) {
-
-      // if string, process as pathplanner trajectory
-      if (inputs[i] instanceof String) {
-
-        PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
-        PathPlannerState initState = (PathPlannerState) traj.sample(0);
-
-        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
-        commands.add(new PPSwerveControllerCommand(
-                          traj,
-                          m_robotSwerveDrive::getOdometry,
-                          m_robotSwerveDrive.m_kinematics,
-                          xController,
-                          yController,
-                          thetaController,
-                          m_robotSwerveDrive::setModuleStates,
-                          m_robotSwerveDrive));
-      }
-
-      // if command, just add it to the array
-      if (inputs[i] instanceof Command) {
-        commands.add((Command) inputs[i]);
-      }
-    }
-    
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
-    Command[] ret = new Command[commands.size()];
-    ret = commands.toArray(ret);
-    return ret;
-  }
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
-    return new ParallelCommandGroup(
-            buildAuto(
-              null,
-              null,
-              new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")), 
-              new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))
-            )
-          );
+    if (loadedPathTrajectory != null) {
+      PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+      PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+      ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+      thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+      PathPlannerState initialState = loadedPathTrajectory.getInitialState();
+      Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
+      return new SequentialCommandGroup(
+          new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
+          new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
+          new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
+              m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
+              m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
+          new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
+    } else {
+      LOGGER.severe("No auto selected.");
+      return new RunCommand(() -> {}).withName("No Autonomous Path");
+    }
   }
 
-  /**
-   * Add your docs here.
-   */
   public XboxController getDriverController() {
     return m_driverXbox;
   }
@@ -235,10 +219,144 @@ public class RobotContainer {
   public void resetOdometry(Pose2d pose) {
     m_robotSwerveDrive.resetOdometry(pose);
   }
-  /**
-   * Add your docs here.
-   */
+
   public XboxController getOperatorController() {
     return m_operatorXbox;
   }
+
+  private void autoInit() {
+    try {
+      WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, StandardWatchEventKinds.ENTRY_DELETE);
+      // TODO: Store this and other commands as fields so they can be rescheduled.
+      new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
+        @Override
+        public boolean runsWhenDisabled() {
+          return true;
+        }
+      }.withName("Path Watcher").schedule();
+    } catch (IOException exception) {
+      LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
+    }
+    Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
+        .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
+        .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+  }
+
+  public void recordInit() {
+    SmartDashboard.putData("Recording",
+        new RunCommand(this::recordPeriodic) {
+          @Override
+          public void end(boolean interupted) {
+            new InstantCommand(RobotContainer.this::saveRecording) {
+              @Override
+              public boolean runsWhenDisabled() {
+                return true;
+              }
+            }.withName("Save Recording").schedule();
+          }
+        }.withName("Record Path (Cancel to Save)"));
+  }
+
+  private void updateAutoChooser(WatchKey watchKey) {
+    List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
+    if (!watchEvents.isEmpty()) {
+      List<WatchEvent<?>> pathWatchEvents = watchEvents.stream().filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
+      for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
+        Path watchEventPath = (Path) pathWatchEvent.context();
+        File watchEventFile = watchEventPath.toFile();
+        String watchEventFileName = watchEventFile.getName();
+        if (watchEventFileName.endsWith(".path")) {
+          if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
+            LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", watchEventFileName);
+            autoChooser.addOption(watchEventFile.getName(), watchEventFile);
+          } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
+            LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
+            if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
+              LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
+              loadPath(watchEventFileName);
+            }
+          } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
+            LOGGER.log(Level.SEVERE, "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", watchEventFileName);
+          }
+        }
+      }
+    }
+    if (!watchKey.reset())
+      LOGGER.severe("File watch key invalid.");
+  }
+
+  private void loadPath(String pathName) {
+    LOGGER.warning("Loading path " + pathName);
+    loadedPathTrajectory = null;
+    loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
+    LOGGER.info("Done loading");
+  }
+
+  private void saveRecording() {
+    // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+    File outputFile = PATHPLANNER_DIRECTORY.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
+    LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
+    if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+      // TODO: Change to use measured maximum velocity and acceleration.
+      var path = createPath(null, null, false);
+      if (RobotBase.isReal())
+        path.write(outputFile);
+      StringWriter writer = new StringWriter();
+      path.write(writer);
+      recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
+      autoChooser.setDefaultOption(outputFile.getName(), outputFile);
+      LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
+    } else
+      LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
+  }
+
+  public void recordPeriodic() {
+    Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
+    Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
+    // FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity.
+    Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
+    Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
+    pathPoints.add(waypoint);
+  }
+
+  public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
+    // Remove points whose angles to neighboring points are less than 10 degrees apart.
+    int j = 0;
+    for (int i = 1; i < pathPoints.size() - 1; i++) {
+      var prev = pathPoints.get(j).anchorPoint.orElseThrow();
+      var current = pathPoints.get(i).anchorPoint.orElseThrow();
+      var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
+      var fromPrevious = current.minus(prev);
+      var toNext = next.minus(current);
+      var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
+      var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
+      if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
+        pathPoints.set(i, null);
+      else
+        j = i;
+    }
+    pathPoints.removeIf(Objects::isNull);
+    // Make control points
+    pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
+    for (int i = 1; i < pathPoints.size() - 1; i++) {
+      var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
+      pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
+      pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
+    }
+    pathPoints.get(pathPoints.size() - 1).prevControl = Optional.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
+    // Create the path
+    PathPlannerUtil.Path path = new PathPlannerUtil.Path();
+    path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
+    path.maxVelocity = Optional.ofNullable(maxVelocity);
+    path.maxAcceleration = Optional.ofNullable(maxAcceleration);
+    path.isReversed = Optional.ofNullable(isReversed);
+    pathPoints.clear();
+    return path;
+  }
+
+  private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current, Translation2d next) {
+    var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
+    return Pair.of(current.minus(line), current.plus(line));
+  }
 }

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -10,6 +10,7 @@ import java.util.Comparator;
 import java.util.Map;
 import java.util.Optional;
 import java.util.function.Function;
+import java.util.logging.Logger;
 import java.util.regex.Pattern;
 import java.util.stream.IntStream;
 
@@ -24,82 +25,81 @@ import frc4388.utility.CSV;
 import frc4388.utility.Gains;
 
 public class BoomBoom extends SubsystemBase {
-public WPI_TalonFX m_shooterFalconLeft;
-public WPI_TalonFX m_shooterFalconRight;
-public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
-public static BoomBoom m_boomBoom;
+  private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
+  public WPI_TalonFX m_shooterFalconLeft;
+  public WPI_TalonFX m_shooterFalconRight;
+  public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+  public static BoomBoom m_boomBoom;
 
-double velP;
-double input;
+  double velP;
+  double input;
 
-public boolean m_isDrumReady = false;
-public double m_fireVel;
+  public boolean m_isDrumReady = false;
+  public double m_fireVel;
 
-public Hood m_hoodSubsystem;
-public Turret m_turretSubsystem;
+  public Hood m_hoodSubsystem;
+  public Turret m_turretSubsystem;
 
-// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
+  // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values
+  // later
 
-public static class ShooterTableEntry {
-  public Double distance, hoodExt, drumVelocity;
-}
-
-private ShooterTableEntry[] m_shooterTable;
-
-/**
- * Creates a new BoomBoom subsystem, which has the drum shooter.
- * @param shooterFalconLeft The left drum motor.
- * @param shooterFalconRight The right drum motor.
- */
-public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
-  m_shooterFalconLeft = shooterFalconLeft;
-  m_shooterFalconRight = shooterFalconRight;
-
-  try {
-    CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
-      private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
-
-      @Override
-      protected String headerSanitizer(final String header) {
-        return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
-      }
-
-    };
-    m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath());
-    new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); 
-  } catch (final IOException e) {
-    e.printStackTrace();
-    // throw new RuntimeException(e);
+  public static class ShooterTableEntry {
+    public Double distance, hoodExt, drumVelocity;
   }
-}
 
-public Double getVelocity(final Double distance) {
-  return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
-}
+  private ShooterTableEntry[] m_shooterTable;
 
-public Double getHood(final Double distance) {
-  return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
-}
+  /*
+  * Creates new BoomBoom subsystem, has drum shooter and angle adjuster
+  */
+  /** Creates a new BoomBoom. */
+  public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
+    m_shooterFalconLeft = shooterFalconLeft;
+    m_shooterFalconRight = shooterFalconRight;
 
-// TODO: @nathanrsxtn pls java doc your shooter tables code
+    try {
+      CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
+        private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
 
-private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
-  final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
-  final E closestRecord = closestEntry.getValue();
-  final int closestRecordIndex = closestEntry.getKey();
-  final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
-  return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
-}
+        @Override
+        protected String headerSanitizer(final String header) {
+          return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
+        }
 
-private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
-  final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
-  return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
-}
+      };
+      m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
+      new Thread(() -> LOGGER.fine(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
+    } catch (final IOException e) {
+      e.printStackTrace();
+      // throw new RuntimeException(e);
+    }
+  }
 
-private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
-  final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
-  return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
-}
+  public Double getVelocity(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
+  }
+
+  public Double getHood(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
+  }
+
+  private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
+    final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
+    final E closestRecord = closestEntry.getValue();
+    final int closestRecordIndex = closestEntry.getKey();
+    final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
+    return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
+  }
+
+  private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
+    final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
+    return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
+  }
+
+  private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
+    final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
+    return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
+  }
 
   @Override
   public void periodic() {
@@ -111,33 +111,34 @@ private static Number lerp2(final Number x, final Number x0, final Number y0, fi
     m_hoodSubsystem = subsystem0;
     m_turretSubsystem = subsystem1;
   }
-/**
- * Runs the Drum motor at a given speed
- * @param speed percent output form -1.0 to 1.0
- */
-public void runDrumShooter(double speed) {
-    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
-    
-  }    
 
-public void setShooterGains() {
-  m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-  m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
-}
+  /**
+   * Runs the Drum motor at a given speed
+   * @param speed percent output form -1.0 to 1.0
+   */
+  public void runDrumShooter(double speed) {
+    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
+
+  }
+
+  public void setShooterGains() {
+    m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
+    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  }
 
   public void runDrumShooterVelocityPID(double targetVel) {
-    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
     m_shooterFalconRight.follow(m_shooterFalconLeft);
-    // New BoomBoom controller stuff 
-    //Controls a motor with the output of the BangBang controller
-    //Controls a motor with the output of the BangBang conroller and a feedforward
-    //Shrinks the feedforward slightly to avoid over speeding the shooter
-    
-    
-    // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel));
+    // New BoomBoom controller stuff
+    // Controls a motor with the output of the BangBang controller
+    // Controls a motor with the output of the BangBang conroller and a feedforward
+    // Shrinks the feedforward slightly to avoid over speeding the shooter
+
+    // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 *
+    // feedforward.calculate(targetVel));
     // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
   }
 }

src/main/java/frc4388/robot/subsystems/LED.java

@@ -27,7 +27,7 @@ public class LED extends SubsystemBase {
     m_LEDController = LEDController;
     setPattern(LEDConstants.DEFAULT_PATTERN);
     updateLED();
-    Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+    Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
   }
 
   @Override

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -4,10 +4,8 @@
 
 package frc4388.robot.subsystems;
 
-import java.util.ArrayList;
-
-import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -19,11 +17,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
@@ -59,6 +55,7 @@ public class SwerveDrive extends SubsystemBase {
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
   public Rotation2d rotTarget = new Rotation2d();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   private final Field2d m_field = new Field2d();
 
@@ -112,6 +109,7 @@ public class SwerveDrive extends SubsystemBase {
               : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
     setModuleStates(states);
   }
+
   public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
   {
     ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
@@ -122,11 +120,12 @@ public class SwerveDrive extends SubsystemBase {
     double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
     double xSpeedMetersPerSecond = -speed.getX();
     double ySpeedMetersPerSecond = speed.getY();
+    chassisSpeeds = fieldRelative
+      ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+      : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
     SwerveModuleState[] states =
         m_kinematics.toSwerveModuleStates(
-            fieldRelative
-              ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
-              : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
+            chassisSpeeds);
     setModuleStates(states);
   }
 
@@ -143,7 +142,7 @@ public class SwerveDrive extends SubsystemBase {
     }
     // modules[0].setDesiredState(desiredStates[0], false);
   }
-  
+
   @Override
   public void periodic() {