diff --git a/simgui-ds.json b/simgui-ds.json
index b16ea5c..0c5f2e8 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -91,10 +91,9 @@
   ],
   "robotJoysticks": [
     {
-      "useGamepad": true
+      "guid": "78696e70757401000000000000000000"
     },
     {
-      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     }
   ]
diff --git a/simgui.json b/simgui.json
index 72aab8f..280daf8 100644
--- a/simgui.json
+++ b/simgui.json
@@ -30,5 +30,13 @@
       "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
       "/LiveWindow/Vision": "Subsystem"
     }
+  },
+  "NetworkTables": {
+    "SmartDashboard": {
+      "Recording": {
+        "open": true
+      },
+      "open": true
+    }
   }
 }
diff --git a/src/main/deploy/Distances.csv b/src/main/deploy/Distances.csv
new file mode 100644
index 0000000..e69de29
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 6dde32e..5d61976 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -31,28 +31,28 @@ public final class Constants {
     public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
     public static final double MAX_SPEED_FEET_PER_SEC = 16;
     public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
-    // public static final int LEFT_FRONT_STEER_CAN_ID = 2;
-    // public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
-    // public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
-    // public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
-    // public static final int LEFT_BACK_STEER_CAN_ID = 6;
-    // public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
-    // public static final int RIGHT_BACK_STEER_CAN_ID = 8;
-    // public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
-    // public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
-    // public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
-    // public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
-    // public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
+    public static final int LEFT_FRONT_STEER_CAN_ID = 2;
+    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
+    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
+    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
+    public static final int LEFT_BACK_STEER_CAN_ID = 6;
+    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
+    public static final int RIGHT_BACK_STEER_CAN_ID = 8;
+    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
+    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
+    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
+    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
+    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
     // ofsets are in degrees
     //ofsets are in degrees
-        // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;    
-        // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;   
-        // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;     
-        // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
-        public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
-        public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
-        public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
-        public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
+        public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;    
+        public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;   
+        public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;     
+        public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
+        // public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
+        // public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
+        // public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
+        // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
 
     // swerve PID constants
     public static final int SWERVE_SLOT_IDX = 0;
@@ -110,8 +110,8 @@ public final class Constants {
     public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
     public static final double SHOOTER_TURRET_MIN = -1.0;
 
-    public static final double DEADZONE_LEFT = -20.0;
-    public static final double DEADZONE_RIGHT = 0.0;
+    public static final double DEADZONE_LEFT = 0.0;
+    public static final double DEADZONE_RIGHT = 340.0;
 
     public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later//
     public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"//
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 119a032..bf9b82c 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -30,6 +30,7 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
+    //AimToCenterTest.RunAll();
     m_robotContainer = new RobotContainer();
   }
 
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 86707d8..29ebda8 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -7,6 +7,7 @@ package frc4388.robot;
 import javax.swing.text.WrappedPlainView;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
@@ -37,124 +38,112 @@ public class RobotMap {
   }
 
   /* Swerve Subsystem */
-  public final WPI_TalonFX leftFrontSteerMotor = null;
-  public final WPI_TalonFX leftFrontWheelMotor = null;
-  public final WPI_TalonFX rightFrontSteerMotor = null;
-  public final WPI_TalonFX rightFrontWheelMotor = null;
-  public final WPI_TalonFX leftBackSteerMotor = null;
-  public final WPI_TalonFX leftBackWheelMotor = null;
-  public final WPI_TalonFX rightBackSteerMotor = null;
-  public final WPI_TalonFX rightBackWheelMotor = null;
-  public final CANCoder leftFrontEncoder = null;
-  public final CANCoder rightFrontEncoder = null;
-  public final CANCoder leftBackEncoder = null;
-  public final CANCoder rightBackEncoder = null;
-  
-  // public final WPI_TalonFX leftFrontSteerMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
-  // public final WPI_TalonFX leftFrontWheelMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
-  // public final WPI_TalonFX rightFrontSteerMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
-  // public final WPI_TalonFX rightFrontWheelMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
-  // public final WPI_TalonFX leftBackSteerMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
-  // public final WPI_TalonFX leftBackWheelMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
-  // public final WPI_TalonFX rightBackSteerMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
-  // public final WPI_TalonFX rightBackWheelMotor = new
-  // WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
-  // public final CANCoder leftFrontEncoder = new
-  // CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
-  // public final CANCoder rightFrontEncoder = new
-  // CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
-  // public final CANCoder leftBackEncoder = new
-  // CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
-  // public final CANCoder rightBackEncoder = new
-  // CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
+  // public final WPI_TalonFX leftFrontSteerMotor = null;
+  // public final WPI_TalonFX leftFrontWheelMotor = null;
+  // public final WPI_TalonFX rightFrontSteerMotor = null;
+  // public final WPI_TalonFX rightFrontWheelMotor = null;
+  // public final WPI_TalonFX leftBackSteerMotor = null;
+  // public final WPI_TalonFX leftBackWheelMotor = null;
+  // public final WPI_TalonFX rightBackSteerMotor = null;
+  // public final WPI_TalonFX rightBackWheelMotor = null;
+  // public final CANCoder leftFrontEncoder = null;
+  // public final CANCoder rightFrontEncoder = null;
+  // public final CANCoder leftBackEncoder = null;
+  // public final CANCoder rightBackEncoder = null;
 
-  // void configureSwerveMotorControllers() {
-  // leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
-  // rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
-  // leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
-  // rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
+  public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
+  public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
+  public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
+  public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
+  public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
+  public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
+  public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
+  public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
+  public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
+  public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
+  public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
+  public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
 
-  // leftFrontSteerMotor.configFactoryDefault();
-  // leftFrontWheelMotor.configFactoryDefault();
-  // rightFrontSteerMotor.configFactoryDefault();
-  // rightFrontWheelMotor.configFactoryDefault();
-  // leftBackSteerMotor.configFactoryDefault();
-  // leftBackWheelMotor.configFactoryDefault();
-  // rightBackSteerMotor.configFactoryDefault();
-  // rightBackWheelMotor.configFactoryDefault();
+  void configureSwerveMotorControllers() {
+  leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
+  rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
+  leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
+  rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
 
-  // leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftFrontSteerMotor.configFactoryDefault();
+  leftFrontWheelMotor.configFactoryDefault();
+  rightFrontSteerMotor.configFactoryDefault();
+  rightFrontWheelMotor.configFactoryDefault();
+  leftBackSteerMotor.configFactoryDefault();
+  leftBackWheelMotor.configFactoryDefault();
+  rightBackSteerMotor.configFactoryDefault();
+  rightBackWheelMotor.configFactoryDefault();
 
-  // leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-  // leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-  // // config cancoder as remote encoder for swerve steer motors
-  // leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
-  // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
-  // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
-  // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
-  // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-  // SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-  // }
+  leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+
+  // config cancoder as remote encoder for swerve steer motors
+  leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
+  RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
+  RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
+  RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
+  RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+  SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  }
 
   // Shooter Config
   /* Boom Boom Subsystem */
diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java
index 723bd88..7b4300f 100644
--- a/src/main/java/frc4388/robot/commands/AimToCenter.java
+++ b/src/main/java/frc4388/robot/commands/AimToCenter.java
@@ -17,6 +17,7 @@ public class AimToCenter extends CommandBase {
   // use odometry to find x and y later
   double x = 0;
   double y = 0;
+  double angle = 0;
   double m_targetAngle;
 
   // public static Gains m_aimGains;
@@ -31,34 +32,26 @@ public class AimToCenter extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    x = 0;
+    y = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (x > 0) {
-      m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle();
-    }
-    else if (x < 0) {
-      m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle();
-    }
-    else if (x == 0 && y > 0) {
-      m_targetAngle = 270 - m_drive.gyro.getAngle();
-    }
-    else if (x == 0 && y < 0) {
-      m_targetAngle = 90 - m_drive.gyro.getAngle();
-    }
-
+    m_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle());
     m_turret.runshooterRotatePID(m_targetAngle);
-
   }
 
-  public boolean isDeadzone() {
-    if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) {
-      return true;
-    } else {
-      return false;
-    }
+  public static double angleToCenter(double x, double y, double gyro) {
+    double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
+    return(angle);
+    //m_turret.runshooterRotatePID(m_targetAngle);
+  }
+
+  public static boolean isDeadzone(double angle) {
+    if ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT)) return true;
+    else return false;
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java
new file mode 100644
index 0000000..8023e72
--- /dev/null
+++ b/src/test/java/frc4388/commands/AimToCenterTest.java
@@ -0,0 +1,113 @@
+package frc4388.commands;
+
+import org.junit.Test;
+
+import frc4388.robot.commands.AimToCenter;
+import org.junit.Assert;
+
+public class AimToCenterTest {
+
+    private static final double DELTA = 1e-15;
+
+    @Test
+    public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
+        boolean output;
+
+            //20 deg
+        output = AimToCenter.isDeadzone(20.);
+        Assert.assertFalse(output);
+
+            //-10 deg
+        output = AimToCenter.isDeadzone(-10.);
+        Assert.assertTrue(output);
+        
+            //-1 deg
+        output = AimToCenter.isDeadzone(-1.);
+        Assert.assertTrue(output);
+
+            //341 deg
+        output = AimToCenter.isDeadzone(341.);
+        Assert.assertTrue(output);
+
+            //340 deg
+        output = AimToCenter.isDeadzone(340.);
+        Assert.assertFalse(output);
+        
+            //0 deg
+        output = AimToCenter.isDeadzone(0.);
+        Assert.assertFalse(output);
+        
+            //200 deg
+        output = AimToCenter.isDeadzone(200.);
+        Assert.assertFalse(output);
+
+            //2000000 deg
+        output = AimToCenter.isDeadzone(2000000.);
+        Assert.assertTrue(output);
+
+            //NaN deg
+        output = AimToCenter.isDeadzone(Double.NaN);
+        Assert.assertFalse(output);
+    }
+
+    @Test 
+    public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() {
+        double actual;
+        double expected;
+
+            //(5,5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(5., 5., 0.);
+        expected = 180. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(-5,5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(-5.0, 5., 0.);
+        expected = 180. + 90. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(-5,-5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(-5.0, -5., 0.);
+        expected = 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(5,-5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(5., -5., 0.);
+        expected = 90. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(0,0) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(0., 0., 0.);
+        Assert.assertNotNull(actual);
+
+            //(5,5) Gyro = 180 deg
+        actual = AimToCenter.angleToCenter(5., 5., 180.);
+        expected = 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(100,100) Gyro = 90 deg
+        actual = AimToCenter.angleToCenter(100., 100., 90.);
+        expected = 90. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(null,5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.);
+        expected = Double.NaN;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(null,null) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.);
+        expected = Double.NaN;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(null,null) Gyro = null deg
+        actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN);
+        expected = Double.NaN;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(5,5) Gyro = -20 deg
+        actual = AimToCenter.angleToCenter(5., -5., -45.);
+        expected = 180.;
+        Assert.assertEquals(expected, actual, DELTA);
+    
+    }
+}
diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
index 87ab85c..ca0e96e 100644
--- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
+++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
@@ -17,23 +17,17 @@ import frc4388.utility.LEDPatterns;
  * Add your docs here.
  */
 public class LEDSubsystemTest {
+  // Arrange
+  Spark ledController = mock(Spark.class);
+  LED led = new LED(ledController);
   @Test
   public void testConstructor() {
-    // Arrange
-    Spark ledController = mock(Spark.class);
-
-    // Act
-    LED led = new LED(ledController);
-
     // Assert
     assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
   }
 
   @Test
   public void testPatterns() {
-    // Arrange
-    Spark ledController = mock(Spark.class);
-    LED led = new LED(ledController);
 
     // Act
     led.setPattern(LEDPatterns.RAINBOW_RAINBOW);