Merge branch 'master' into vision-april-lime

2026-06-09 00:37:59 -06:00 · 2023-02-27 17:37:08 -07:00
parent 92243d0942 61f5911e48
commit deaf36df0a
14 changed files with 957 additions and 90 deletions
@@ -47,18 +47,17 @@ public final class Constants {
    public static final class PIDConstants {
      public static final int SWERVE_SLOT_IDX = 0;
      public static final int SWERVE_PID_LOOP_IDX = 1;
-      public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
+      public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
    }

    public static final class AutoConstants {
-      public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
-      public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
-      public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, 
-        new TrapezoidProfile.Constraints(0.0, 0.0)
-      );
+      public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+      public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+      public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
+      public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune

-      public static final double PATH_MAX_VEL = -1; // TODO: find the actual value
-      public static final double PATH_MAX_ACC = -1; // TODO: find the actual value
+      public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
+      public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
    }

    public static final class Conversions {
@@ -89,7 +88,8 @@ public final class Constants {
      public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
    }

-    public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
+    public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
+    public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value

    // dimensions
    public static final double WIDTH = 18.5;
@@ -10,8 +10,13 @@ package frc4388.robot;
 import java.lang.System;
 import java.lang.reflect.Array;
 import java.util.Arrays;
+import java.io.File;
+import java.io.IOException;
+import java.io.PrintWriter;

+import edu.wpi.first.wpilibj.DriverStation;
 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 frc4388.utility.RobotTime;
@@ -45,6 +50,8 @@ public class Robot extends TimedRobot {
    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
    // autonomous chooser on the dashboard.
    m_robotContainer = new RobotContainer();
+
+    SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
  }

  /**
@@ -94,6 +101,7 @@ public class Robot extends TimedRobot {
   */
  @Override
  public void autonomousInit() {
+    m_robotContainer.m_robotSwerveDrive.resetGyro();
    m_autonomousCommand = m_robotContainer.getAutonomousCommand();

    // schedule the autonomous command (example)
@@ -112,6 +120,8 @@ public class Robot extends TimedRobot {

  @Override
  public void teleopInit() {
+    m_robotContainer.m_robotSwerveDrive.resetGyro();
+
    // This makes sure that the autonomous stops running when
    // teleop starts running. If you want the autonomous to
    // continue until interrupted by another command, remove
@@ -119,6 +129,7 @@ public class Robot extends TimedRobot {
    if (m_autonomousCommand != null) {
      m_autonomousCommand.cancel();
    }
+    m_robotContainer.m_robotSwerveDrive.resetGyro();
    m_robotTime.startMatchTime();
  }

@@ -7,15 +7,42 @@

 package frc4388.robot;

+import java.util.ArrayList;
+import java.util.List;
+
+import org.opencv.objdetect.HOGDescriptor;
+
+import edu.wpi.first.math.controller.HolonomicDriveController;
+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.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.Trajectory.State;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+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.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
+import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
 import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.commands.DriveWithInput;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.controller.IHandController;
+import frc4388.robot.commands.JoystickPlayback;
+import frc4388.robot.commands.JoystickRecorder;
+import frc4388.robot.commands.PlaybackChooser;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController;

 /**
@@ -30,31 +57,60 @@ public class RobotContainer {
    public final RobotMap m_robotMap = new RobotMap();

    /* Subsystems */
-    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
-    // private final LED m_robotLED = new LED(m_robotMap.LEDController);
-    
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+                                                                  m_robotMap.rightFront,
+                                                                  m_robotMap.leftBack,
+                                                                  m_robotMap.rightBack,
+                                                                  m_robotMap.gyro);

    /* Controllers */
-    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
-    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+    private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
+    private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);

+    /* Autos */
+    public SendableChooser<Command> chooser = new SendableChooser<>();
+    
+    private Command noAuto = new InstantCommand();
+    
+    // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
+    
+    // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
+    // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
+    
+    // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
+    // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
+
+    // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
+
+    private PlaybackChooser playbackChooser;
    /**
     * The container for the robot. Contains subsystems, OI devices, and commands.
     */
    public RobotContainer() {
        configureButtonBindings();

-        /* Default Commands */
-
-        m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, 
-                                                                () -> getDriverController().getLeftXAxis(), 
-                                                                () -> getDriverController().getLeftYAxis(), 
-                                                                () -> getDriverController().getRightXAxis(),
-                                                                false));
+        // * Default Commands
+        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+                m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+                                                  getDeadbandedDriverController().getRight(),
+                                                  true);
+            }, m_robotSwerveDrive)
+            .withName("SwerveDrive DefaultCommand"));
        
-        // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+        // * Auto Commands
+        // chooser.setDefaultOption("NoAuto", noAuto);

-    
+        // chooser.addOption("Blue1Path", blue1Path);
+        // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
+
+        // chooser.addOption("Red1Path", red1Path);
+        // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
+        
+        // chooser.addOption("Taxi", taxi);
+
+        playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
+            .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
+            .buildDisplay();
    }


@@ -65,20 +121,27 @@ public class RobotContainer {
     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
     */
    private void configureButtonBindings() {
-        /* Driver Buttons */
+        // * Driver Buttons
+        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
        
-        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
-            // .onFalse()
-
-        /* Operator Buttons */
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+        //     // .onFalse()

        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));

-        // interrupt button
-        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand());
+        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
+                                            () -> getDeadbandedDriverController().getLeftX(),
+                                            () -> getDeadbandedDriverController().getLeftY(),
+                                            () -> getDeadbandedDriverController().getRightX(),
+                                            () -> getDeadbandedDriverController().getRightY(),
+                                            "Blue1Path.txt"))
+            .onFalse(new InstantCommand());
+
+        // * Operator Buttons
    }

    /**
@@ -87,35 +150,16 @@ public class RobotContainer {
     * @return the command to run in autonomous
     */
    public Command getAutonomousCommand() {
-
-        return new InstantCommand();
+        // return chooser.getSelected();
+        return playbackChooser.getCommand();
    }

-    /**
-     * Add your docs here.
-     */
-    public IHandController getDriverController() {
-        return m_driverXbox;
+    public DeadbandedXboxController getDeadbandedDriverController() {
+        return this.m_driverXbox;
    }

-    /**
-     * Add your docs here.
-     */
-    public IHandController getOperatorController() {
-        return m_operatorXbox;
+    public DeadbandedXboxController getDeadbandedOperatorController() {
+        return this.m_operatorXbox;
    }

-    /**
-     * Add your docs here.
-     */
-    public Joystick getOperatorJoystick() {
-        return m_operatorXbox.getJoyStick();
-    }
-
-    /**
-     * Add your docs here.
-     */
-    public Joystick getDriverJoystick() {
-        return m_driverXbox.getJoyStick();
-    }
 }
@@ -25,7 +25,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {

 	@Override
 	public double getError() {
-		var pitch = gyro.getPitch();
+		var pitch = gyro.getRoll();
 		SmartDashboard.putNumber("pitch", pitch);
 		return pitch;
 	}
@@ -33,8 +33,9 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 	@Override
 	public void runWithOutput(double output) {
 		double out2 = MathUtil.clamp(output / 40, -.5, .5);
-		if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
-		drive.drive(out2, 0, 0, false);
+
+		if (Math.abs(getError()) < 3) out2 = 0;
+		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
 	}

 	@Override
@@ -0,0 +1,150 @@
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,2
+0.0,0.0,0.0,0.0,18
+0.0,0.0,0.0,0.0,31
+0.0,0.0,0.0,0.0,61
+0.0,0.0,0.0,0.0,72
+0.0,0.0,0.0,0.0,85
+0.0,0.0,0.0,0.0,102
+0.0,0.0,0.0,0.0,116
+0.0,0.0,0.0,0.0,163
+0.0,0.0,0.0,0.0,175
+0.0,0.0,0.0,0.0,188
+0.0,0.0,0.0,0.0,205
+0.0,0.0,0.0,0.0,217
+0.0,0.0,0.0,0.0,229
+0.0,0.0,0.0,0.0,241
+0.0,0.0,0.0,0.0,261
+0.0,0.0,0.0,0.0,281
+0.0,0.0,0.0,0.0,302
+0.0,0.0,0.0,0.0,322
+0.0,0.0,0.0,0.0,341
+0.0,0.0,0.0,0.0,361
+0.0,-0.1328125,0.0,0.0,397
+0.0,-0.1328125,0.0,0.0,412
+0.0,-0.1484375,0.0,0.0,425
+0.0,-0.1796875,0.0,0.0,441
+0.0,-0.1875,0.0,0.0,461
+0.0,-0.1953125,0.0,0.0,481
+0.0,-0.234375,0.0,0.0,502
+0.0,-0.2890625,0.0,0.0,521
+0.0,-0.3125,0.0,0.0,541
+0.0,-0.328125,0.0,0.0,561
+0.0,-0.3671875,0.0,0.0,582
+0.0,-0.390625,0.0,0.0,601
+0.0,-0.4609375,0.0,0.0,642
+0.0,-0.4765625,0.0,0.0,653
+0.0,-0.4765625,0.0,0.0,666
+0.0,-0.4765625,0.0,0.0,681
+0.0,-0.4765625,0.0,0.0,702
+0.0,-0.4765625,0.0,0.0,721
+0.0,-0.4765625,0.0,0.0,741
+0.0,-0.4765625,0.0,0.0,762
+0.0,-0.4765625,0.0,0.0,782
+0.0,-0.484375,0.0,0.0,803
+0.0,-0.484375,0.0,0.0,821
+0.0,-0.4921875,0.0,0.0,842
+0.0,-0.5078125,0.0,0.0,878
+0.0,-0.5234375,0.0,0.0,893
+0.0,-0.5234375,0.0,0.0,906
+0.0,-0.53125,0.0,0.0,922
+0.0,-0.53125,0.0,0.0,942
+0.0,-0.53125,0.0,0.0,962
+0.0,-0.53125,0.0,0.0,982
+0.0,-0.5390625,0.0,0.0,1001
+0.0,-0.5390625,0.0,0.0,1022
+0.0,-0.546875,0.0,0.0,1042
+0.0,-0.546875,0.0,0.0,1061
+0.0,-0.546875,0.0,0.0,1082
+0.0,-0.546875,0.0,0.0,1164
+0.0,-0.546875,0.0,0.0,1176
+0.0,-0.546875,0.0,0.0,1190
+0.0,-0.546875,0.0,0.0,1202
+0.0,-0.546875,0.0,0.0,1219
+0.0,-0.546875,0.0,0.0,1230
+0.0,-0.546875,0.0,0.0,1244
+0.0,-0.546875,0.0,0.0,1258
+0.0,-0.546875,0.0,0.0,1268
+0.0,-0.546875,0.0,0.0,1281
+0.0,-0.546875,0.0,0.0,1301
+0.0,-0.546875,0.0,0.0,1321
+0.0,-0.546875,0.0,0.0,1363
+0.0,-0.546875,0.0,0.0,1376
+0.0,-0.546875,0.0,0.0,1388
+0.0,-0.546875,0.0,0.0,1402
+0.0,-0.546875,0.0,0.0,1422
+0.0,-0.546875,0.0,0.0,1441
+0.0,-0.546875,0.0,0.0,1461
+0.0,-0.546875,0.0,0.0,1482
+0.0,-0.546875,0.0,0.0,1502
+0.0,-0.546875,0.0,0.0,1521
+0.0,-0.546875,0.0,0.0,1542
+0.0,-0.546875,0.0,0.0,1562
+0.0,-0.546875,0.0,0.0,1598
+0.0,-0.546875,0.0,0.0,1609
+0.0,-0.546875,0.0,0.0,1621
+0.0,-0.546875,0.0,0.0,1642
+0.0,-0.546875,0.0,0.0,1661
+0.0,-0.546875,0.0,0.0,1682
+0.0,-0.546875,0.0,0.0,1702
+0.0,-0.546875,0.0,0.0,1722
+0.0,-0.546875,0.0,0.0,1742
+0.0,-0.546875,0.0,0.0,1762
+0.0,-0.546875,0.0,0.0,1781
+0.0,-0.5390625,0.0,0.0,1801
+0.0,-0.5390625,0.0,0.0,1835
+0.0,-0.5390625,0.0,0.0,1847
+0.0,-0.5390625,0.0,0.0,1861
+0.0,-0.5390625,0.0,0.0,1882
+0.0,-0.5390625,0.0,0.0,1901
+0.0,-0.5390625,0.0,0.0,1921
+0.0,-0.5390625,0.0,0.0,1941
+0.0,-0.5390625,0.0,0.0,1962
+0.0,-0.5390625,0.0,0.0,1983
+0.0,-0.5390625,0.0,0.0,2001
+0.0,-0.5390625,0.0,0.0,2022
+0.0,-0.5390625,0.0,0.0,2042
+0.0,-0.5390625,0.0,0.0,2085
+0.0,-0.5390625,0.0,0.0,2097
+0.0,-0.5390625,0.0,0.0,2113
+0.0,-0.5390625,0.0,0.0,2124
+0.0,-0.5390625,0.0,0.0,2141
+0.0,-0.5390625,0.0,0.0,2161
+0.0,-0.5390625,0.0,0.0,2181
+0.0,-0.5390625,0.0,0.0,2201
+0.0,-0.5390625,0.0,0.0,2221
+0.0,-0.5390625,0.0,0.0,2241
+0.0,-0.5390625,0.0,0.0,2262
+0.0,-0.5390625,0.0,0.0,2283
+0.0,-0.2265625,0.0,0.0,2377
+0.0,-0.2265625,0.0,0.0,2390
+0.0,0.0,0.0,0.0,2402
+0.0,0.0,0.0,0.0,2417
+0.0,0.0,0.0,0.0,2428
+0.0,0.0,0.0,0.0,2440
+0.0,0.0,0.0,0.0,2451
+0.0,0.0,0.0,0.0,2463
+0.0,0.0,0.0,0.0,2475
+0.0,0.0,0.0,0.0,2493
+0.0,0.0,0.0,0.0,2505
+0.0,0.0,0.0,0.0,2521
+0.0,0.0,0.0,0.0,2558
+0.0,0.0,0.0,0.0,2576
+0.0,0.0,0.0,0.0,2592
+0.0,0.0,0.0,0.0,2603
+0.0,0.0,0.0,0.0,2621
+0.0,0.0,0.0,0.0,2641
+0.0,0.0,0.0,0.0,2661
+0.0,0.0,0.0,0.0,2681
+0.0,0.0,0.0,0.0,2702
+0.0,0.0,0.0,0.0,2721
+0.0,0.0,0.0,0.0,2742
+0.0,0.0,0.0,0.0,2762
+0.0,0.0,0.0,0.0,2805
+0.0,0.0,0.0,0.0,2814
+0.0,0.0,0.0,0.0,2825
+0.0,0.0,0.0,0.0,2841
+0.0,0.0,0.0,0.0,2861
+0.0,0.0,0.0,0.0,2882
+0.0,0.0,0.0,0.0,2901
+0.0,0.0,0.0,0.0,2922
@@ -0,0 +1,141 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.util.ArrayList;
+import java.util.Scanner;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.UtilityStructs.TimedOutput;
+
+public class JoystickPlayback extends CommandBase {
+  private final SwerveDrive            swerve;
+  private       String                 filename;
+  private       int                    mult       = 1;
+  private       Scanner                input;
+  private final ArrayList<TimedOutput> outputs      = new ArrayList<>();
+  private       int                    counter      = 0;
+  private       long                   startTime    = 0;
+  private       long                   playbackTime = 0;
+  private       int                    lastIndex;
+  private       boolean                m_finished   = false; // ! find a better way
+
+  /** Creates a new JoystickPlayback. */
+  public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
+    this.swerve = swerve;
+    this.filename = filename;
+    this.mult = mult;
+
+    addRequirements(this.swerve);
+  }
+
+  /** Creates a new JoystickPlayback. */
+  public JoystickPlayback(SwerveDrive swerve, String filename) {
+    this(swerve, filename, 1);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    outputs.clear();
+    m_finished = false;
+
+    startTime = System.currentTimeMillis();
+    playbackTime = 0;
+    lastIndex    = 0;
+    try {
+      input = new Scanner(new File("/home/lvuser/autos/" + filename));
+
+      String line = "";
+      while (input.hasNextLine()) {
+        line = input.nextLine();
+
+        if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
+          continue;
+        }
+      
+        String[] values = line.split(",");
+        System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
+
+        var out = new TimedOutput();
+        out.leftX  = Double.parseDouble(values[0]) * mult;
+        out.leftY  = Double.parseDouble(values[1]);
+        out.rightX = Double.parseDouble(values[2]);
+        out.rightY = Double.parseDouble(values[3]);
+
+        out.timedOffset = Long.parseLong(values[4]);
+
+        outputs.add(out);
+      }
+
+      input.close();
+    } catch (FileNotFoundException e) {
+      e.printStackTrace();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (counter == 0) {
+      startTime = System.currentTimeMillis();
+      playbackTime = 0;
+    } else {
+      playbackTime = System.currentTimeMillis() - startTime;
+    }
+
+    // skip to reasonable time frame
+    // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
+    {
+      int i = lastIndex == 0 ? 1 : lastIndex;
+      while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
+        i++;
+      }
+
+      if (i >= outputs.size()) {
+        m_finished = true; // ! kind of a hack
+        return;
+      }
+      lastIndex = i;
+    }
+
+    TimedOutput lastOut = outputs.get(lastIndex - 1);
+    TimedOutput out     = outputs.get(lastIndex);
+
+    double deltaTime     = out.timedOffset - lastOut.timedOffset;
+    double playbackDelta = playbackTime    - lastOut.timedOffset;
+
+    double lerpLX = lastOut.leftX  + (out.leftX  - lastOut.leftX)  * (playbackDelta / deltaTime);
+    double lerpLY = lastOut.leftY  + (out.leftY  - lastOut.leftY)  * (playbackDelta / deltaTime);
+    double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
+    double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
+
+    // this.swerve.driveWithInput(new Translation2d(out.leftX,   out.leftY),
+    //                              new Translation2d(out.rightX, out.rightY),
+    //                              true);
+    
+    this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
+                                new Translation2d(lerpRX, lerpRY),
+                                true);
+                             
+    counter++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    input.close();
+    swerve.stopModules();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_finished;
+  }
+}
@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.UtilityStructs.TimedOutput;
+
+public class JoystickRecorder extends CommandBase {
+  public  final SwerveDrive            swerve;
+
+  public  final Supplier<Double>       leftX;
+  public  final Supplier<Double>       leftY;
+  public  final Supplier<Double>       rightX;
+  public  final Supplier<Double>       rightY;
+  private       String                 filename;
+  public  final ArrayList<TimedOutput> outputs   = new ArrayList<>();
+  private       long                   startTime = -1;
+
+
+  /** Creates a new JoystickRecorder. */
+  public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX,  Supplier<Double> leftY,
+                                              Supplier<Double> rightX, Supplier<Double> rightY,
+                                              String filename)
+  {
+    this.swerve = swerve;
+    this.leftX  = leftX;
+    this.leftY  = leftY;
+    this.rightX = rightX;
+    this.rightY = rightY;
+    this.filename = filename;
+
+    addRequirements(this.swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    outputs.clear();
+
+    this.startTime = System.currentTimeMillis();
+
+    outputs.add(new TimedOutput());
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    var inputs = new TimedOutput();
+    inputs.leftX       = leftX.get();
+    inputs.leftY       = leftY.get();
+    inputs.rightX      = rightX.get();
+    inputs.rightY      = rightY.get();
+    inputs.timedOffset = System.currentTimeMillis() - startTime;
+
+    outputs.add(inputs);
+
+    swerve.driveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
+                          new Translation2d(inputs.rightX, inputs.rightY),
+                          true);
+    
+    System.out.println("RECORDING");
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    File output = new File("/home/lvuser/autos/" + filename);
+
+    try (PrintWriter writer = new PrintWriter(output)) {
+      for (var input : outputs) {
+        writer.println( input.leftX  + "," + input.leftY  + "," +
+                        input.rightX + "," + input.rightY + "," +
+                        input.timedOffset);
+      }
+
+      writer.close();
+    } catch (IOException e) {
+        e.printStackTrace();
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
@@ -0,0 +1,80 @@
+package frc4388.robot.commands;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.WidgetType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class PlaybackChooser {
+    private final ArrayList<SendableChooser<Command>> m_choosers    = new ArrayList<>();
+    private       SendableChooser<Command>            m_playback    = null;
+    private final HashMap<String, Command>            m_commandPool = new HashMap<>();
+
+    private final File        m_dir = new File("/home/lvuser/autos/");
+    private final SwerveDrive m_swerve;
+
+    // commands
+    private Command m_noAuto = new InstantCommand();
+    
+    public PlaybackChooser(SwerveDrive swerve, Object... pool) {
+        m_swerve = swerve;
+    }
+
+    public PlaybackChooser addOption(String name, Command option) {
+        m_commandPool.put(name, option);
+        return this;
+    }
+
+    public PlaybackChooser buildDisplay() {
+        appendCommand();
+        m_playback = m_choosers.get(0);
+
+        Shuffleboard.getTab("Auto Chooser")
+            .add("Add Sequence", new InstantCommand(() -> appendCommand()))
+            .withPosition(4, 0);
+        return this;
+    }
+
+    // This will be bound to a button for the time being
+    public void appendCommand() {
+        var chooser = new SendableChooser<Command>();
+
+        chooser.setDefaultOption("No Auto", m_noAuto);
+        for (String auto : m_dir.list()) {
+            m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto));
+        }
+        for (var cmd_name : m_commandPool.keySet()) {
+            chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+        }
+
+        m_choosers.add(chooser);
+        Shuffleboard.getTab("Auto Chooser")
+            .add("Command: " + m_choosers.size(), chooser)
+            .withSize(4, 1)
+            .withPosition(0, m_choosers.size() - 1)
+            .withWidget(BuiltInWidgets.kSplitButtonChooser);
+    }
+
+    public Command getCommand() {
+        Command command = m_playback.getSelected();
+        command = command == null ? m_noAuto : command.asProxy();
+        
+        Command[] commands = new Command[m_choosers.size() - 1];
+        for (int i = 0; i < m_choosers.size()-1; i++) {
+            Command command2 = m_choosers.get(i + 1).getSelected();
+            command2 = command2 == null ? m_noAuto : command2.asProxy();
+
+            commands[i] = command2.asProxy();
+        }
+
+        return command.andThen(commands);
+    }
+}
@@ -0,0 +1,225 @@
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,12
+0.0,0.0,0.0,0.0,26
+0.0,0.0,0.0,0.0,37
+0.0,0.0,0.0,0.0,50
+0.0,0.0,0.0,0.0,62
+0.0,0.0,0.0,0.0,73
+0.0,0.0,0.0,0.0,88
+0.0,0.0,0.0,0.0,103
+0.0,0.0,0.0,0.0,116
+0.0,0.0,0.0,0.0,160
+0.0,0.0,0.0,0.0,173
+0.0,0.0,0.0,0.0,185
+0.0,0.0,0.0,0.0,200
+0.0,0.0,0.0,0.0,211
+0.0,0.0,0.0,0.0,223
+0.0,0.0,0.0,0.0,235
+0.0,0.0,0.0,0.0,247
+0.0,0.0,0.0,0.0,263
+0.0,0.0,0.0,0.0,283
+0.0,0.0,0.0,0.0,303
+0.0,-0.109375,0.0,0.0,323
+0.0,-0.1484375,0.0,0.0,343
+0.0,-0.2109375,0.0,0.0,363
+0.0,-0.3671875,0.0,0.0,398
+0.0,-0.4140625,0.0,0.0,411
+0.0,-0.4765625,0.0,0.0,425
+0.0,-0.5078125,0.0,0.0,443
+0.0,-0.5078125,0.0,0.0,463
+0.0,-0.53125,0.0,0.0,483
+0.0,-0.5546875,0.0,0.0,504
+0.0,-0.5625,0.0,0.0,523
+0.0,-0.5625,0.0,0.0,544
+0.0,-0.5703125,0.0,0.0,563
+0.0,-0.5859375,0.0,0.0,584
+0.0,-0.5859375,0.0,0.0,603
+0.0,-0.5859375,0.0,0.0,640
+0.0,-0.59375,0.0,0.0,657
+0.0,-0.6015625,0.0,0.0,672
+0.0,-0.6015625,0.0,0.0,685
+0.0,-0.6015625,0.0,0.0,703
+0.0,-0.6015625,0.0,0.0,723
+0.0,-0.6015625,0.0,0.0,743
+0.0,-0.6015625,0.0,0.0,763
+0.0,-0.6015625,0.0,0.0,783
+0.0,-0.6015625,0.0,0.0,803
+0.0,-0.6015625,0.0,0.0,823
+0.0,-0.6015625,0.0,0.0,844
+0.0,-0.6015625,0.0,0.0,878
+0.0,-0.6015625,0.0,0.0,893
+0.0,-0.6015625,0.0,0.0,907
+0.0,-0.6015625,0.0,0.0,924
+0.0,-0.609375,0.0,0.0,943
+0.0,-0.609375,0.0,0.0,963
+0.0,-0.609375,0.0,0.0,983
+0.0,-0.609375,0.0,0.0,1004
+0.0,-0.609375,0.0,0.0,1023
+0.0,-0.609375,0.0,0.0,1043
+0.0,-0.609375,0.0,0.0,1064
+0.0,-0.609375,0.0,0.0,1083
+0.0,-0.609375,0.0,0.0,1156
+0.0,-0.609375,0.0,0.0,1172
+0.0,-0.609375,0.0,0.0,1185
+0.0,-0.609375,0.0,0.0,1200
+0.0,-0.609375,0.0,0.0,1215
+0.0,-0.609375,0.0,0.0,1225
+0.0,-0.609375,0.0,0.0,1236
+0.0,-0.609375,0.0,0.0,1249
+0.0,-0.609375,0.0,0.0,1263
+0.0,-0.609375,0.0,0.0,1283
+0.0,-0.609375,0.0,0.0,1303
+0.0,-0.609375,0.0,0.0,1323
+0.0,-0.609375,0.0,0.0,1363
+0.0,-0.6015625,0.0,0.0,1376
+0.0,-0.6015625,0.0,0.0,1394
+0.0,-0.6015625,0.0,0.0,1405
+0.0,-0.6015625,0.0,0.0,1423
+0.0,-0.6015625,0.0,0.0,1443
+0.0,-0.6015625,0.0,0.0,1463
+0.0,-0.6015625,0.0,0.0,1483
+0.0,-0.6015625,0.0,0.0,1503
+0.0,-0.6015625,0.0,0.0,1523
+0.0,-0.6015625,0.0,0.0,1543
+0.0,-0.6015625,0.0,0.0,1563
+0.0,-0.6015625,0.0,0.0,1597
+0.0,-0.6015625,0.0,0.0,1608
+0.0,-0.6015625,0.0,0.0,1624
+0.0,-0.6015625,0.0,0.0,1643
+0.0,-0.6015625,0.0,0.0,1664
+0.0,-0.5859375,0.0,0.0,1683
+0.0,-0.5859375,0.0,0.0,1703
+0.0,-0.5625,0.0,0.0,1723
+0.0,-0.5625,0.0,0.0,1743
+0.0,-0.5625,0.0,0.0,1763
+0.0,-0.5625,0.0,0.0,1783
+0.0,-0.5625,0.0,0.0,1803
+0.0,-0.5625,0.0,0.0,1843
+0.0,-0.5625,0.0,0.0,1855
+0.0,-0.5625,0.0,0.0,1868
+0.0,-0.5625,0.0,0.0,1883
+0.0,-0.5625,0.0,0.0,1903
+0.0,-0.5625,0.0,0.0,1923
+0.0,-0.5625,0.0,0.0,1943
+0.0,-0.5625,0.0,0.0,1963
+0.0,-0.5625,0.0,0.0,1983
+0.0,-0.5625,0.0,0.0,2003
+0.0,-0.5625,0.0,0.0,2024
+0.0,-0.5625,0.0,0.0,2043
+0.0,-0.5625,0.0,0.0,2081
+0.0,-0.5625,0.0,0.0,2093
+0.0,-0.5625,0.0,0.0,2105
+0.0,-0.5625,0.0,0.0,2123
+0.0,-0.5625,0.0,0.0,2143
+0.0,-0.5625,0.0,0.0,2163
+0.0,-0.5625,0.0,0.0,2183
+0.0,-0.5625,0.0,0.0,2203
+0.0,-0.5625,0.0,0.0,2223
+0.0,-0.5625,0.0,0.0,2243
+0.0,-0.5625,0.0,0.0,2263
+0.0,-0.5625,0.0,0.0,2283
+0.0,-0.5625,0.0,0.0,2366
+0.0,-0.5625,0.0,0.0,2377
+0.0,-0.5625,0.0,0.0,2394
+0.0,-0.5703125,0.0,0.0,2405
+0.0,-0.5703125,0.0,0.0,2418
+0.0,-0.5703125,0.0,0.0,2431
+0.0,-0.5703125,0.0,0.0,2444
+0.0,-0.5703125,0.0,0.0,2458
+0.0,-0.5703125,0.0,0.0,2470
+0.0,-0.5703125,0.0,0.0,2485
+0.0,-0.5703125,0.0,0.0,2503
+0.0,-0.5703125,0.0,0.0,2523
+0.0,-0.5703125,0.0,0.0,2563
+0.0,-0.5703125,0.0,0.0,2577
+0.0,-0.5703125,0.0,0.0,2591
+0.0,-0.5703125,0.0,0.0,2608
+0.0,-0.5703125,0.0,0.0,2624
+0.0,-0.5703125,0.0,0.0,2643
+0.0,-0.5703125,0.0,0.0,2677
+0.0,-0.5703125,0.0,0.0,2698
+0.0,-0.5703125,0.0,0.0,2711
+0.0,-0.5703125,0.0,0.0,2725
+0.0,-0.5703125,0.0,0.0,2743
+0.0,-0.5703125,0.0,0.0,2764
+0.0,-0.5703125,0.0,0.0,2810
+0.0,-0.5703125,0.0,0.0,2820
+0.0,-0.5703125,0.0,0.0,2833
+0.0,-0.5703125,0.0,0.0,2845
+0.0,-0.5703125,0.0,0.0,2863
+0.0,-0.5703125,0.0,0.0,2883
+0.0,-0.5703125,0.0,0.0,2904
+0.0,-0.5703125,0.0,0.0,2924
+0.0,-0.5703125,0.0,0.0,2943
+0.0,-0.5703125,0.0,0.0,2963
+0.0,-0.5703125,0.0,0.0,2983
+0.0,-0.5703125,0.0,0.0,3003
+0.0,-0.5703125,0.0,0.0,3033
+0.0,-0.5703125,0.0,0.0,3050
+0.0,-0.5703125,0.0,0.0,3065
+0.0,-0.5703125,0.0,0.0,3083
+0.0,-0.5703125,0.0,0.0,3103
+0.0,-0.5703125,0.0,0.0,3123
+0.0,-0.5703125,0.0,0.0,3144
+0.0,-0.5703125,0.0,0.0,3164
+0.0,-0.5703125,0.0,0.0,3184
+0.0,-0.5703125,0.0,0.0,3203
+0.0,-0.5703125,0.0,0.0,3223
+0.0,-0.5703125,0.0,0.0,3243
+0.0,-0.5703125,0.0,0.0,3272
+0.0,-0.5703125,0.0,0.0,3289
+0.0,-0.5703125,0.0,0.0,3303
+0.0,-0.5703125,0.0,0.0,3323
+0.0,-0.5703125,0.0,0.0,3343
+0.0,-0.5703125,0.0,0.0,3363
+0.0,-0.5703125,0.0,0.0,3383
+0.0,-0.5703125,0.0,0.0,3403
+0.0,-0.5703125,0.0,0.0,3423
+0.0,-0.5703125,0.0,0.0,3443
+0.0,-0.5703125,0.0,0.0,3463
+0.0,-0.5703125,0.0,0.0,3483
+0.0,-0.5703125,0.0,0.0,3566
+0.0,-0.5703125,0.0,0.0,3578
+0.0,-0.5703125,0.0,0.0,3596
+0.0,-0.5703125,0.0,0.0,3610
+0.0,-0.5703125,0.0,0.0,3623
+0.0,-0.5703125,0.0,0.0,3640
+0.0,-0.5703125,0.0,0.0,3651
+0.0,-0.5703125,0.0,0.0,3663
+0.0,-0.5703125,0.0,0.0,3678
+0.0,-0.5703125,0.0,0.0,3691
+0.0,-0.5703125,0.0,0.0,3706
+0.0,-0.5703125,0.0,0.0,3723
+0.0,-0.5703125,0.0,0.0,3766
+0.0,-0.5703125,0.0,0.0,3778
+0.0,-0.5703125,0.0,0.0,3792
+0.0,-0.5703125,0.0,0.0,3807
+0.0,-0.5703125,0.0,0.0,3823
+0.0,-0.5703125,0.0,0.0,3843
+0.0,-0.53125,0.0,0.0,3863
+0.0,-0.53125,0.0,0.0,3884
+0.0,-0.421875,0.0,0.0,3904
+0.0,0.0,0.0,0.0,3924
+0.0,0.0,0.0,0.0,3944
+0.0,0.0,0.0,0.0,3963
+0.0,0.0,0.0,0.0,3999
+0.0,0.0,0.0,0.0,4010
+0.0,0.0,0.0,0.0,4025
+0.0,0.0,0.0,0.0,4043
+0.0,0.0,0.0,0.0,4063
+0.0,0.0,0.0,0.0,4083
+0.0,0.0,0.0,0.0,4103
+0.0,0.0,0.0,0.0,4123
+0.0,0.0,0.0,0.0,4143
+0.0,0.0,0.0,0.0,4163
+0.0,0.0,0.0,0.0,4183
+0.0,0.0,0.0,0.0,4203
+0.0,0.0,0.0,0.0,4236
+0.0,0.0,0.0,0.0,4247
+0.0,0.0,0.0,0.0,4264
+0.0,0.0,0.0,0.0,4284
+0.0,0.0,0.0,0.0,4304
+0.0,0.0,0.0,0.0,4324
+0.0,0.0,0.0,0.0,4343
+0.0,0.0,0.0,0.0,4363
@@ -4,6 +4,9 @@

 package frc4388.robot.subsystems;

+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+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.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -26,18 +29,13 @@ public class SwerveDrive extends SubsystemBase {
  private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
  private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
  
-  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); 
+  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);

-  // private SwerveDriveOdometry odometry = new SwerveDrive(
-  //   kinematics, 
-  //   gyro.getRotation2d(),
-  //   new SwerveModulePosition[] {
-  //     leftFront.getPosition(),
-  //     rightFront.getPosition(),
-  //     leftBack.getPosition(),
-  //     rightBack.getPosition()
-  //   }
-  // );
+  private RobotGyro gyro;
+
+  // private SwerveDriveOdometry odometry;
+
+  private SwerveDrivePoseEstimator poseEstimator;

  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default

@@ -47,18 +45,55 @@ public class SwerveDrive extends SubsystemBase {
    this.rightFront = rightFront;
    this.leftBack = leftBack;
    this.rightBack = rightBack;
+    
+    this.gyro = gyro;
+
+    // this.odometry = new SwerveDriveOdometry(
+    //   kinematics, 
+    //   gyro.getRotation2d(),
+    //   new SwerveModulePosition[] {
+    //     leftFront.getPosition(),
+    //     rightFront.getPosition(),
+    //     leftBack.getPosition(),
+    //     rightBack.getPosition()
+    //   },
+    //   getOdometry()
+    // );
+
+    this.poseEstimator = new SwerveDrivePoseEstimator(
+      kinematics, 
+      gyro.getRotation2d(), 
+      new SwerveModulePosition[] {
+        leftFront.getPosition(),
+        rightFront.getPosition(),
+        leftBack.getPosition(),
+        rightBack.getPosition()
+      },
+      new Pose2d(0,0, new Rotation2d(0))
+    );

    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
  }

-  // WPILib swerve drive example
-  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    // SwerveModuleState[] states = kinematics.toSwerveModuleStates(
-    //   fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) 
-    //                 : new ChassisSpeeds(xSpeed, ySpeed, rot)
-    // );
-    SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
-    setModuleStates(states);
+  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
+      if (rightStick.getNorm() > 0.1) {
+        rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
+      }
+
+      double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
+
+      // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
+      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+
+      // Convert field-relative speeds to robot-relative speeds.
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+
+    } else {
+      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
  }

  /**
@@ -74,6 +109,16 @@ public class SwerveDrive extends SubsystemBase {
    }
  }

+  public double getGyroAngle() {
+    return gyro.getAngle();
+  }
+
+  public void resetGyro() {
+    gyro.reset();
+    // setOdometry(getOdometry());
+    rotTarget = new Rotation2d(0);
+  }
+
  /**
   * Updates the odometry of the SwerveDrive.
   */
@@ -89,6 +134,18 @@ public class SwerveDrive extends SubsystemBase {
  //   );
  // }

+  public void updatePoseEstimator() {
+    poseEstimator.update(
+      gyro.getRotation2d(), 
+      new SwerveModulePosition[] {
+        leftFront.getPosition(),
+        rightFront.getPosition(),
+        leftBack.getPosition(),
+        rightBack.getPosition()
+      }
+    );
+  }
+
  /**
   * Gets the odometry of the SwerveDrive.
   * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
@@ -97,6 +154,10 @@ public class SwerveDrive extends SubsystemBase {
  //   return odometry.getPoseMeters();
  // }

+  public Pose2d getPoseEstimator() {
+    return poseEstimator.getEstimatedPosition();
+  }
+
  /**
   * Sets the odometry of the SwerveDrive.
   * @param pose Pose to set the odometry to.
@@ -114,21 +175,35 @@ public class SwerveDrive extends SubsystemBase {
  //   );
  // }

+  public void setPoseEstimator(Pose2d pose) {
+    poseEstimator.resetPosition(
+      gyro.getRotation2d(), 
+      new SwerveModulePosition[] {
+        leftFront.getPosition(),
+        rightFront.getPosition(),
+        leftBack.getPosition(),
+        rightBack.getPosition()
+      },
+      pose
+    );
+  }
+
+  public void resetPoseEstimator() {
+    setPoseEstimator(new Pose2d());
+  }
+  
+  public void stopModules() {
+    for (SwerveModule module : this.modules) {
+      module.stop();
+    }
+  }
+
  /**
   * Resets the odometry of the SwerveDrive to 0.
   * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
   */
  // public void resetOdometry() {
-  //   odometry.resetPosition(
-  //     gyro.getRotation2d(), 
-  //     new SwerveModulePosition[] {
-  //       leftFront.getPosition(),
-  //       rightFront.getPosition(),
-  //       leftBack.getPosition(),
-  //       rightBack.getPosition()
-  //     },
-  //     new Pose2d()
-  //   );
+  //   setOdometry(new Pose2d());
  // }

  public SwerveDriveKinematics getKinematics() {
@@ -138,7 +213,16 @@ public class SwerveDrive extends SubsystemBase {
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
+
    // updateOdometry();
+    updatePoseEstimator();
+
+    // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
+    // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
+    // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
+
+    // SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
+    // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
  }

  /**
@@ -124,7 +124,10 @@ public class SwerveModule extends SubsystemBase {

        // convert the CANCoder from its position reading to ticks
        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
-        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+
+        if (!ignoreAngle) {
+            angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+        }

        double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);