diff --git a/config/LeftBackCANCoder.json b/config/LeftBackCANCoder.json
new file mode 100644
index 0000000..69ab1f3
--- /dev/null
+++ b/config/LeftBackCANCoder.json
@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -240.0293,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/config/LeftFrontCANCoder.json b/config/LeftFrontCANCoder.json
new file mode 100644
index 0000000..1a5edcd
--- /dev/null
+++ b/config/LeftFrontCANCoder.json
@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -181.230469,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/config/RightBackCANCoder.json b/config/RightBackCANCoder.json
new file mode 100644
index 0000000..236a112
--- /dev/null
+++ b/config/RightBackCANCoder.json
@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -40.86914,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/config/RightFrontCANCoder.json b/config/RightFrontCANCoder.json
new file mode 100644
index 0000000..ec93eb5
--- /dev/null
+++ b/config/RightFrontCANCoder.json
@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -270.615234,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..73cc713
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
new file mode 100644
index 0000000..59ccda3
--- /dev/null
+++ b/simgui.json
@@ -0,0 +1,75 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
+    "Retained Values": {
+      "open": false
+    },
+    "Transitory Values": {
+      "open": false
+    },
+    "retained": {
+      "apriltag": {
+        "open": true
+      }
+    },
+    "transitory": {
+      "LiveWindow": {
+        ".status": {
+          "open": true
+        }
+      },
+      "Shuffleboard": {
+        ".recording": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "NT3@192.168.1.132:40708": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "NT3@192.168.1.132:49546": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "NT3@192.168.1.132:52724": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "shuffleboard": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "visible": true
+  }
+}
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3508a8d..ce15b92 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -23,49 +23,54 @@ import frc4388.utility.LEDPatterns;
  */
 public final class Constants {
   public static final class SwerveDriveConstants {
+
+    public static final double ROTATION_SPEED = -0.7;
+
     public static final class IDs {
-      public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID
-      public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID
-      public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
-      public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find the actual ID
+      public static final int LEFT_FRONT_WHEEL_ID = 2;
+      public static final int LEFT_FRONT_STEER_ID = 3;
+      public static final int LEFT_FRONT_ENCODER_ID = 10;
 
-      public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find the actual ID
-      public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find the actual ID
-      public static final int LEFT_BACK_STEER_ID = -1; // TODO: find the actual ID
-      public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
+      public static final int RIGHT_FRONT_WHEEL_ID = 4;
+      public static final int RIGHT_FRONT_STEER_ID = 5;
+      public static final int RIGHT_FRONT_ENCODER_ID = 11;
 
-      public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID
-      public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID
-      public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
-      public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
+      public static final int LEFT_BACK_WHEEL_ID = 6;
+      public static final int LEFT_BACK_STEER_ID = 7;
+      public static final int LEFT_BACK_ENCODER_ID = 12;
+      
+      public static final int RIGHT_BACK_WHEEL_ID = 8;
+      public static final int RIGHT_BACK_STEER_ID = 9;
+      public static final int RIGHT_BACK_ENCODER_ID = 13;
     }
 
     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 {
       public static final int CANCODER_TICKS_PER_ROTATION = 4096;
 
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8;
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
+
+      public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
+      public static final double MOTOR_REV_PER_STEER_REV = 12.8;
 
-      public static final double MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find the actual value
       public static final double TICKS_PER_MOTOR_REV = 2048;
-      public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
+      public static final double WHEEL_DIAMETER_INCHES = 3.9;
       public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
 
       public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
@@ -81,18 +86,14 @@ public final class Constants {
       public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
       public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
       public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
-
-      public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value
-      public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value
-      public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value
-      public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value
     }
 
-    public static final double MAX_SPEED_FEET_PER_SECOND = -1; // 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 = -1; // TODO: find the actual value
-    public static final double HEIGHT = -1; // TODO: find the actual value
+    public static final double WIDTH = 18.5;
+    public static final double HEIGHT = 18.5;
     public static final double HALF_WIDTH = WIDTH / 2.d;
     public static final double HALF_HEIGHT = HEIGHT / 2.d;
 
@@ -102,7 +103,13 @@ public final class Constants {
   }
 
   public static final class GyroConstants {
-    public static final int ID = -1; // TODO: find the actual ID
+    public static final int ID = 14; // TODO: find the actual ID
+  }
+  
+  public static final class ArmConstants {
+      public static final int MIN_ARM_LEN = 0;
+      public static final int MAX_ARM_LEN = 1;
+      public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
   }
 
   public static final class ArmConstants {
@@ -125,9 +132,9 @@ public final class Constants {
 
     public static final double OFFSET = 0;
   }
-    
+
   public static final class LEDConstants {
-    public static final int LED_SPARK_ID = 0;
+    // public static final int LED_SPARK_ID = 0;
 
     public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
   }
@@ -135,5 +142,8 @@ public final class Constants {
   public static final class OIConstants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
+    public static final double LEFT_AXIS_DEADBAND = 0.1;
+    public static final double RIGHT_AXIS_DEADBAND = 0.6;
+    public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
   }
 }
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 3f93b48..ac78650 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,19 +7,24 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
+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 edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.commands.AutoBalance;
-import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
+import frc4388.robot.subsystems.Location;
+import frc4388.robot.subsystems.Apriltags.Tag;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the TimedRobot
@@ -33,34 +38,8 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
-  public static class MicroBot extends SubsystemBase {
-    public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
-    public RobotGyro gyro = new RobotGyro(pigeon);
+  private Location location = new Location();
 
-    public TalonSRX frontLeft = new TalonSRX(2);
-    public TalonSRX backLeft = new TalonSRX(3);
-    public TalonSRX backRight = new TalonSRX(5);
-    public TalonSRX frontRight = new TalonSRX(4);
-
-    public MicroBot() {
-      frontRight.configFactoryDefault();
-      frontLeft.configFactoryDefault();
-      backLeft.configFactoryDefault();
-      backRight.configFactoryDefault();
-  
-      frontLeft.setInverted(true);
-      backLeft.setInverted(true);
-    }
-
-    public void setOutput(double output) {
-      frontRight.set(ControlMode.PercentOutput, output);
-      frontLeft.set(ControlMode.PercentOutput, output);
-      backLeft.set(ControlMode.PercentOutput, output);
-      backRight.set(ControlMode.PercentOutput, output);
-    }
-  }
-
-  private MicroBot bot = new MicroBot();
 
   /**
    * This function is run when the robot is first started up and should be
@@ -72,7 +51,7 @@ public class Robot extends TimedRobot {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    bot.setDefaultCommand(new AutoBalance(bot));
+    SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
   }
 
   /**
@@ -91,6 +70,13 @@ public class Robot extends TimedRobot {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    final Tag pos = location.getPosRot();
+    if (pos != null) {
+      SmartDashboard.putNumber("x position", pos.x);
+    }
+
+    //ystem.out.print(apriltagPos[0]);
   }
 
   /**
@@ -112,15 +98,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
+    m_robotContainer.m_robotSwerveDrive.resetGyro();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    /*
-     * String autoSelected = SmartDashboard.getString("Auto Selector",
-     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-     * = new MyAutoCommand(); break; case "Default Auto": default:
-     * autonomousCommand = new ExampleCommand(); break; }
-     */
-
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
@@ -137,6 +117,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
@@ -144,9 +126,8 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    m_robotContainer.m_robotSwerveDrive.resetGyro();
     m_robotTime.startMatchTime();
-
-    m_robotContainer.gyroRef.reset();
   }
 
   /**
@@ -154,9 +135,6 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
-    SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
-    SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a7f47c7..7c897c3 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -7,18 +7,43 @@
 
 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.subsystems.Arm;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
+import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.LEDPatterns;
-import frc4388.utility.RobotGyro;
 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,43 +55,69 @@ import frc4388.utility.controller.XboxController;
  */
 public class RobotContainer {
     /* RobotMap */
-    private final RobotMap m_robotMap = new RobotMap();
+    public final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro);
     private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
     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);
 
-    public RobotGyro gyroRef = m_robotMap.gyro;
+    /* 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 */
-        // drives the robot with a two-axis input from the driver controller
-        // continually sends updates to the Blinkin LED controller to keep the lights on
-        m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+        // * Default Commands
+        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+                m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+                                                  getDeadbandedDriverController().getRight(),
+                                                  true);
+            }, m_robotSwerveDrive)
+            .withName("SwerveDrive DefaultCommand"));
+        
+        // * Auto Commands
+        // chooser.setDefaultOption("NoAuto", noAuto);
 
-        m_robotSwerveDrive.setDefaultCommand(
-            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), 
-                                                                    getDriverController().getLeftYAxis(), 
-                                                                   -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)
-        );
+        // chooser.addOption("Blue1Path", blue1Path);
+        // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
+        // chooser.addOption("Red1Path", red1Path);
+        // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
+        
+        // chooser.addOption("Taxi", taxi);
 
-        m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.armSetLength(
-            getOperatorController().getLeftYAxis()), m_robotArm)
-        );
+        playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
+            .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
+            .buildDisplay();
     }
 
+
     /**
      * Use this method to define your button->command mappings. Buttons can be
      * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -74,20 +125,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(() -> gyroRef.reset()));
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+        //     // .onFalse()
 
-        /* Operator Buttons */
-        // activates "Lit Mode"
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
-            .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
 
-        //New interupt 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
     }
 
     /**
@@ -96,35 +154,16 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // no auto
-        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();
-    }
 }
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 0d9ee69..e454e52 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -9,10 +9,10 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -40,7 +40,7 @@ public class RobotMap {
   }
 
   /* LED Subsystem */
-  public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+  // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
   void configureLEDMotorControllers() {
         
@@ -115,19 +115,18 @@ public class RobotMap {
 
     rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
     rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
-
-    // config magnet offset
-    leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
-    rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET);
-    leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET);
-    rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
+    
+    // set neutral mode
+    leftFrontSteer.setNeutralMode(NeutralMode.Brake);
+    rightFrontSteer.setNeutralMode(NeutralMode.Brake);
+    leftBackSteer.setNeutralMode(NeutralMode.Brake);
+    rightBackSteer.setNeutralMode(NeutralMode.Brake);
 
     // initialize SwerveModules
-    this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
-    this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET);
-    this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET);
-    this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
-
+    this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
+    this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
+    this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
+    this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
   }
 
   // arm stuff
diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java
index a8cff7d..2da529c 100644
--- a/src/main/java/frc4388/robot/commands/AutoBalance.java
+++ b/src/main/java/frc4388/robot/commands/AutoBalance.java
@@ -5,37 +5,44 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
-import frc4388.robot.Robot;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.RobotGyro;
 
-// NOTE:	Consider using this command inline, rather than writing a subclass.	For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class AutoBalance extends PelvicInflammatoryDisease {
-	Robot.MicroBot bot;
+	RobotGyro gyro;
+	SwerveDrive drive;
 
 	/** Creates a new AutoBalanceTF2. */
-	public AutoBalance(Robot.MicroBot bot) {
-		super(.7, .1, 15, 0);
-		addRequirements(bot);
-		this.bot = bot;
+	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
+		super(0.6, 0, 0, 0);
+
+		this.gyro = gyro;
+		this.drive = drive;
+
+		addRequirements(drive);
 	}
 
 	@Override
 	public double getError() {
-		return bot.gyro.getPitch();
+		var pitch = gyro.getRoll();
+		SmartDashboard.putNumber("pitch", pitch);
+		return pitch;
 	}
 
 	@Override
 	public void runWithOutput(double output) {
 		double out2 = MathUtil.clamp(output / 40, -.5, .5);
-		if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
-		bot.setOutput(out2);
+
+		if (Math.abs(getError()) < 3) out2 = 0;
+		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
 	}
 
 	@Override
 	public void initialize() {
 		super.initialize();
-		this.bot.gyro.reset();
+		// this.gyro.reset();
 	}
 
 	// Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/commands/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Blue1Path.txt
new file mode 100644
index 0000000..70d9ee7
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/Blue1Path.txt
@@ -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
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
new file mode 100644
index 0000000..0f496a9
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
@@ -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;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java
new file mode 100644
index 0000000..eea72b4
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java
@@ -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;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
index 1ab08d5..fb6a871 100644
--- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
@@ -11,7 +11,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 	protected Gains gains;
 	private double output = 0;
 
-	/** Creates a new PelvicInflamitoryDisease. */
+	/** Creates a new PelvicInflammatoryDisease. */
 	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
 		gains = new Gains(kp, ki, kd, kf, 0);
 	}
diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java
new file mode 100644
index 0000000..af7f4ac
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java
@@ -0,0 +1,90 @@
+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.ComplexWidget;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+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 ArrayList<ComplexWidget>            m_widgets     = new ArrayList<>();
+    private final HashMap<String, Command>            m_commandPool = new HashMap<>();
+    
+    private final File        m_dir    = new File("/home/lvuser/autos/");
+    private       int         m_cmdNum = 0;
+    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() {
+        for (int i = 0; i < 10; i++) {
+            appendCommand();
+        }
+        m_playback = m_choosers.get(0);
+        nextChooser();
+
+        Shuffleboard.getTab("Auto Chooser")
+            .add("Add Sequence", new InstantCommand(() -> {}))
+            .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);
+
+        m_choosers.add(chooser);
+        ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
+            .add("Command: " + m_choosers.size(), chooser)
+            .withSize(4, 1)
+            .withPosition(0, m_choosers.size() - 1)
+            .withWidget(BuiltInWidgets.kSplitButtonChooser);
+
+        m_widgets.add(widget);
+    }
+
+    public void nextChooser() {
+        SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
+
+        for (String auto : m_dir.list()) {
+            chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+        }
+        for (var cmd_name : m_commandPool.keySet()) {
+            chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+        }
+    }
+
+    public Command getCommand() {
+        Command command = m_playback.getSelected();
+        command = command == null ? m_noAuto : command.asProxy();
+        
+        Command[] commands = new Command[m_cmdNum - 1];
+        for (int i = 0; i < m_cmdNum - 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);
+    }
+}
diff --git a/src/main/java/frc4388/robot/commands/Taxi.txt b/src/main/java/frc4388/robot/commands/Taxi.txt
new file mode 100644
index 0000000..c99ee2c
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/Taxi.txt
@@ -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
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java
new file mode 100644
index 0000000..c6062e8
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java
@@ -0,0 +1,36 @@
+package frc4388.robot.subsystems;
+
+//import edu.wpi.first.apriltag.AprilTag;
+//import edu.wpi.first.math.geometry.Pose3d;
+//import edu.wpi.first.math.geometry.Rotation3d;
+//import edu.wpi.first.networktables.NetworkTable;
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Apriltags {
+    public static class Tag {
+        public boolean visible = true;
+        public double x,  y,  z  = 0;
+        public double ry, rp, rr = 0;
+    }
+
+    public Tag getTagPosRot() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        final Tag tag = new Tag();
+        tag.visible = isAprilTag();
+        tag.x  = tagTable.getEntry("TagPosX").getDouble(0);
+        tag.y  = tagTable.getEntry("TagPosY").getDouble(0);
+        tag.z  = tagTable.getEntry("TagPosZ").getDouble(0);
+        tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
+        tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
+        tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
+
+        return tag;
+    }
+
+    public boolean isAprilTag() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+        return tagTable.getEntry("IsTag").getBoolean(false);
+    }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 513be03..ca7b058 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -7,7 +7,6 @@ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
-
 import frc4388.robot.Constants.ArmConstants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java
new file mode 100644
index 0000000..37793b1
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Claw.java
@@ -0,0 +1,23 @@
+package frc4388.robot.subsystems;
+import edu.wpi.first.wpilibj.PWM;
+
+public class Claw {
+	private PWM m_clawMotor;
+	private boolean m_open = false;
+
+    // Opens claw
+	public Claw(PWM m_clawMotor) {
+		 this.m_clawMotor = m_clawMotor;
+		 setClaw(true);
+	}
+
+    public void setClaw(boolean open) {
+        // Open claw
+		m_open = open;
+		m_clawMotor.setRaw(open ? 0 : 2000);
+    }
+
+	public boolean isClawOpen() {
+		return m_open;
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java
new file mode 100644
index 0000000..0bf5322
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Location.java
@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems;
+
+import frc4388.robot.subsystems.Apriltags.Tag;
+
+public class Location {
+    final Apriltags apriltag = new Apriltags();
+
+    private boolean isLimelight = false;
+    private boolean isApriltag = false;
+
+    //Determines which source to get pos and rot from and also resets
+    private void reoderPrio(){
+        isLimelight = false; //If limelight gets position and if within a certain range of poles
+        isApriltag = apriltag.isAprilTag();
+    }
+
+    public Tag getPosRot() {
+        reoderPrio();
+        if(isApriltag){
+            return apriltag.getTagPosRot();
+        } else if (isLimelight) {
+            return null;
+        }
+
+        return null;
+    }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 676d800..ac27c3b 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -4,15 +4,16 @@
 
 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;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
 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.math.kinematics.SwerveModulePosition;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.RobotGyro;
 
@@ -25,27 +26,23 @@ public class SwerveDrive extends SubsystemBase {
 
   private SwerveModule[] modules;
 
-  private RobotGyro gyro;
-
   private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   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 SwerveDriveOdometry(
-    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
+  
+  public Rotation2d rotTarget = new Rotation2d();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   /** Creates a new SwerveDrive. */
   public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
@@ -53,23 +50,55 @@ public class SwerveDrive extends SubsystemBase {
     this.rightFront = rightFront;
     this.leftBack = leftBack;
     this.rightBack = rightBack;
-
-    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
-
+    
     this.gyro = gyro;
-  }
 
-  public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-    double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+    // this.odometry = new SwerveDriveOdometry(
+    //   kinematics, 
+    //   gyro.getRotation2d(),
+    //   new SwerveModulePosition[] {
+    //     leftFront.getPosition(),
+    //     rightFront.getPosition(),
+    //     leftBack.getPosition(),
+    //     rightBack.getPosition()
+    //   },
+    //   getOdometry()
+    // );
 
-    SwerveModuleState[] states = kinematics.toSwerveModuleStates(
-      fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) 
-                    : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
+    this.poseEstimator = new SwerveDrivePoseEstimator(
+      kinematics, 
+      gyro.getRotation2d(), 
+      new SwerveModulePosition[] {
+        leftFront.getPosition(),
+        rightFront.getPosition(),
+        leftBack.getPosition(),
+        rightBack.getPosition()
+      },
+      new Pose2d(0,0, new Rotation2d(0))
     );
 
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-    setModuleStates(states);
+    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
+  }
+
+  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));
   }
 
   /**
@@ -77,6 +106,7 @@ public class SwerveDrive extends SubsystemBase {
    * @param desiredStates Array of module states to set.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
     for (int i = 0; i < desiredStates.length; i++) {
       SwerveModule module = modules[i];
       SwerveModuleState state = desiredStates[i];
@@ -84,11 +114,33 @@ 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.
    */
-  public void updateOdometry() {
-    odometry.update(
+  // public void updateOdometry() {
+  //   odometry.update(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     }
+  //   );
+  // }
+
+  public void updatePoseEstimator() {
+    poseEstimator.update(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -103,16 +155,33 @@ public class SwerveDrive extends SubsystemBase {
    * Gets the odometry of the SwerveDrive.
    * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
    */
-  public Pose2d getOdometry() {
-    return odometry.getPoseMeters();
+  // public Pose2d getOdometry() {
+  //   return odometry.getPoseMeters();
+  // }
+
+  public Pose2d getPoseEstimator() {
+    return poseEstimator.getEstimatedPosition();
   }
 
   /**
    * Sets the odometry of the SwerveDrive.
    * @param pose Pose to set the odometry to.
    */
-  public void setOdometry(Pose2d pose) {
-    odometry.resetPosition(
+  // public void setOdometry(Pose2d pose) {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     pose
+  //   );
+  // }
+
+  public void setPoseEstimator(Pose2d pose) {
+    poseEstimator.resetPosition(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -124,27 +193,41 @@ public class SwerveDrive extends SubsystemBase {
     );
   }
 
+  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.
+   * *NOTE: If you reset your gyro, 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()
-    );
+  // public void resetOdometry() {
+  //   setOdometry(new Pose2d());
+  // }
+
+  public SwerveDriveKinematics getKinematics() {
+    return this.kinematics;
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    updateOdometry();
+
+    // 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());
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index ea39ba4..1ddab51 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -10,7 +10,6 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -23,15 +22,15 @@ import frc4388.utility.Gains;
 public class SwerveModule extends SubsystemBase {
     private WPI_TalonFX driveMotor;
     private WPI_TalonFX angleMotor;
-    private CANCoder canCoder;
+    private CANCoder encoder;
 
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
   
     /** Creates a new SwerveModule. */
-    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
+    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
-        this.canCoder = canCoder;
+        this.encoder = encoder;
 
         TalonFXConfiguration angleConfig = new TalonFXConfiguration();
         angleConfig.slot0.kP = swerveGains.kP;
@@ -39,14 +38,15 @@ public class SwerveModule extends SubsystemBase {
         angleConfig.slot0.kD = swerveGains.kD;
 
         // use the CANcoder as the remote sensor for the primary TalonFX PID
-        angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
+        angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
         angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
         angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
         angleMotor.configAllSettings(angleConfig);
 
-        CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
-        canCoderConfig.magnetOffsetDegrees = offset;
-        canCoder.configAllSettings(canCoderConfig);
+        encoder.configMagnetOffset(offset);
+        
+        driveMotor.setSelectedSensorPosition(0);
+        driveMotor.config_kP(0, 0.2);
     }
 
     /**
@@ -70,7 +70,7 @@ public class SwerveModule extends SubsystemBase {
      * @return the CANcoder of the SwerveModule
      */
     public CANCoder getEncoder() {
-        return this.canCoder;
+        return this.encoder;
     }
 
     /**
@@ -78,8 +78,20 @@ public class SwerveModule extends SubsystemBase {
      * @return the angle of a SwerveModule as a Rotation2d
      */
     public Rotation2d getAngle() {
-        // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. 
-        return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
+        // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
+        return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
+    }
+    
+    public double getAngularVel() {
+        return this.angleMotor.getSelectedSensorVelocity();
+    }
+
+    public double getDrivePos() {
+        return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+    }
+
+    public double getDriveVel() {
+        return this.driveMotor.getSelectedSensorVelocity(0);
     }
 
     public void stop() {
@@ -126,15 +138,17 @@ public class SwerveModule extends SubsystemBase {
         double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
 
         // convert the CANCoder from its position reading to ticks
-        double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
+        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+
         angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
 
         double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-        driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
+
+        driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
     }
 
-    public void reset() {
-        canCoder.setPositionToAbsolute();
+    public void reset(double position) {
+        encoder.setPositionToAbsolute();
     }
 
     public double getCurrent() {
@@ -144,4 +158,4 @@ public class SwerveModule extends SubsystemBase {
     public double getVoltage() {
         return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
     }
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java
new file mode 100644
index 0000000..3731849
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Vision.java
@@ -0,0 +1,39 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Vision {
+    private final NetworkTableEntry m_isTags;
+    private final NetworkTableEntry m_xPoses;
+    private final NetworkTableEntry m_yPoses;
+    private final NetworkTableEntry m_zPoses;
+
+    public Vision() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        m_isTags = tagTable.getEntry("IsTag");
+        m_xPoses = tagTable.getEntry("TagPosX");
+        m_yPoses = tagTable.getEntry("TagPosY");
+        m_zPoses = tagTable.getEntry("TagPosZ");
+    }
+
+    public AprilTag[] getAprilTags() {
+        if (!m_isTags.getBoolean(false)) return new AprilTag[0];
+
+        double xarr[] = m_xPoses.getDoubleArray(new double[] {});
+        double yarr[] = m_yPoses.getDoubleArray(new double[] {});
+        double zarr[] = m_zPoses.getDoubleArray(new double[] {});
+
+        AprilTag tags[] = new AprilTag[xarr.length];
+        for (int i = 0; i < tags.length; i++) {
+            tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
+        }
+
+        return tags;
+    }
+}
diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java
new file mode 100644
index 0000000..c2ad269
--- /dev/null
+++ b/src/main/java/frc4388/utility/AprilTag.java
@@ -0,0 +1,13 @@
+package frc4388.utility;
+
+// This is a seperate class in case I want to encode rotation or other
+// information about the tag
+public class AprilTag {
+    public final double x, y, z;
+
+    public AprilTag(double _x, double _y, double _z) {
+        x = _x;
+        y = _y;
+        z = _z;
+    }
+}
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
index 7cda1e0..7a3a026 100644
--- a/src/main/java/frc4388/utility/Gains.java
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -54,6 +54,12 @@ public class Gains {
         this.kMinOutput = -1.0;
     }
 
+    public Gains(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
     /**
      * Creates Gains object for PIDs
      * @param kP The P value.
diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java
new file mode 100644
index 0000000..e8b10cc
--- /dev/null
+++ b/src/main/java/frc4388/utility/UtilityStructs.java
@@ -0,0 +1,12 @@
+package frc4388.utility;
+
+public class UtilityStructs {
+    public static class TimedOutput {
+        public double leftX  = 0.0;
+        public double leftY  = 0.0;
+        public double rightX = 0.0;
+        public double rightY = 0.0;
+    
+        public long timedOffset = 0;
+    }
+}
diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
new file mode 100644
index 0000000..bad1605
--- /dev/null
+++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
@@ -0,0 +1,27 @@
+package frc4388.utility.controller;
+
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+import frc4388.robot.Constants.OIConstants;
+
+public class DeadbandedXboxController extends XboxController {
+  public DeadbandedXboxController(int port) { super(port); }
+  
+  @Override public double getLeftX() { return getLeft().getX(); }
+  @Override public double getLeftY() { return getLeft().getY(); }
+  @Override public double getRightX() { return getRight().getX(); }
+  @Override public double getRightY() { return getRight().getY(); }
+
+  public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
+  public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
+
+  public static Translation2d skewToDeadzonedCircle(double x, double y) {
+    Translation2d translation2d = new Translation2d(x, y);
+    double magnitude = translation2d.getNorm();
+    if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
+    if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
+    return translation2d;
+  }
+}
\ No newline at end of file