diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index cbdb153..a8c1155 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -149,8 +149,21 @@ public static class Presets {
     public static final double ELEVATOR_AMP_HANDOFF = 0.04;
 
     // TODO: get values for these
-    public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap() {};
-    public static final InterpolatingDoubleTreeMap ELEVATOR_DISTANCE_HEIGHT = new InterpolatingDoubleTreeMap() {};
+    public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap();
+    
+    static {
+      SHOOTER_DISTANCE_ANGLE.put(1.755, 43.78);
+
+      SHOOTER_DISTANCE_ANGLE.put(3.500, 28.27);
+    };
+
+    public static final InterpolatingDoubleTreeMap ELEVATOR_DISTANCE_HEIGHT = new InterpolatingDoubleTreeMap();
+
+    static {
+      ELEVATOR_DISTANCE_HEIGHT.put(1.755, 0.061);
+
+      ELEVATOR_DISTANCE_HEIGHT.put(3.500, 0.010);
+    }
   }
 
   public static class Ports {
@@ -168,7 +181,7 @@ public static class FieldConstants {
 
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
 
-    public static final double TAG_DISTANCE_THRESHOLD = 3;
+    public static final double TAG_DISTANCE_THRESHOLD = 3.5;
     public static final double TAG_DISTANCE_RESET_THRESHOLD = 1;
 
     public static final int SPEAKER_TAG_BLUE = 7;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 19ef0cf..2f54c2b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -135,7 +135,7 @@ private void configureBindings() {
     _operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP).handleInterrupt(stopShooter));
     _operatorController.R2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.INTAKE).handleInterrupt(stopShooter));
 
-    _operatorController.square().whileTrue(safeFeedIn);
+    _operatorController.square().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE));
     _operatorController.circle().whileTrue(feedOut);
     _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
     _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, FeedMode.INTAKE));
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index 615cedb..5f4711e 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -166,6 +166,9 @@ public void periodic() {
     SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
     SmartDashboard.putNumber("Shot Distance", shotVector().getNorm());
 
+    SmartDashboard.putNumber("ROBOT X POSE", getPose().getX());
+    SmartDashboard.putNumber("ROBOT Y POSE", getPose().getY());
+
     // Update the bot's pose
     _estimator.update(getHeadingRaw(), new SwerveModulePosition[]{
       _frontLeft.getPosition(),