diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java
index 59ceb49..3f54b61 100644
--- a/src/main/java/frc/robot/driver/DriverXbox.java
+++ b/src/main/java/frc/robot/driver/DriverXbox.java
@@ -211,8 +211,8 @@ public void setupTeleopButtons() {
         controller.leftTrigger().whileTrue(new SetElevatorState(ElevatorSubsystem.State.kAlgaeIntake));
 
 
-        controller.povLeft().whileTrue(mLineupMaster.directDriveToNearestLeftBranch());
-        controller.povRight().whileTrue(mLineupMaster.directDriveToNearestRightBranch());
+        // controller.povLeft().whileTrue(mLineupMaster.directDriveToNearestLeftBranch());
+        // controller.povRight().whileTrue(mLineupMaster.directDriveToNearestRightBranch());
         
 
         //Align to Reef
diff --git a/src/main/java/frc/robot/vision/LineupMaster.java b/src/main/java/frc/robot/vision/LineupMaster.java
index 94ec9b8..756c90f 100644
--- a/src/main/java/frc/robot/vision/LineupMaster.java
+++ b/src/main/java/frc/robot/vision/LineupMaster.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import frc.crevolib.math.Conversions;
@@ -26,13 +27,13 @@ public class LineupMaster {
     private static final Map<ReefFace, Command> leftBranchAlignmentCommands = new HashMap<>();
     private static final Map<ReefFace, Command> reefCenterAlignmentCommands = new HashMap<>();
     private static final Map<ReefFace, Command> rightBranchAlignmentCommands = new HashMap<>();
-    public static final Transform2d robotOffset = new Transform2d(0.508, 0, Rotation2d.kZero);
+    public static final Transform2d robotOffset = new Transform2d(0.3018, 0, Rotation2d.kZero);
     public static final PathConstraints pathConstraints = new PathConstraints(2, 2, Units.degreesToRadians(360), Units.degreesToRadians(360));
     public LineupMaster() {
         for (ReefFace face : ReefFace.values()) {
-            leftBranchAlignmentCommands.put(face, directDriveToPose(Conversions.rotatePose(face.leftBranch.transformBy(robotOffset), Rotation2d.k180deg)));
-            reefCenterAlignmentCommands.put(face, directDriveToPose(Conversions.rotatePose(face.AprilTag.transformBy(robotOffset), Rotation2d.k180deg)));
-            rightBranchAlignmentCommands.put(face, directDriveToPose(Conversions.rotatePose(face.rightBranch.transformBy(robotOffset), Rotation2d.k180deg)));
+            leftBranchAlignmentCommands.put(face, directDriveToPose(Conversions.rotatePose(face.leftBranch.transformBy(robotOffset), Rotation2d.kZero)));
+            reefCenterAlignmentCommands.put(face, directDriveToPose(Conversions.rotatePose(face.AprilTag.transformBy(robotOffset), Rotation2d.kZero)));
+            rightBranchAlignmentCommands.put(face, directDriveToPose(Conversions.rotatePose(face.rightBranch.transformBy(robotOffset), Rotation2d.kZero)));
         }
     }
 
@@ -48,6 +49,8 @@ public static ReefFace getClosestReefFace(Pose2d robotPose){
             }
         }
 
+        SmartDashboard.putString("closest reef face", closestFace.name());
+
         return closestFace;
     }
 
diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java
index d3a471a..5c4a46d 100644
--- a/src/main/java/frc/robot/vision/VisionConfig.java
+++ b/src/main/java/frc/robot/vision/VisionConfig.java
@@ -384,14 +384,14 @@ public static class AlignmentConfig {
     }
     
     private static final double branchOffset = Units.inchesToMeters(6.469);
-    private static final Transform2d leftBranchTransform = new Transform2d(0.0, -branchOffset, Rotation2d.kZero);
-    private static final Transform2d rightBranchTransform = new Transform2d(0.0, branchOffset, Rotation2d.kZero);
+    private static final Transform2d leftBranchTransform = new Transform2d(0.0, -Units.inchesToMeters(6), Rotation2d.kZero);
+    private static final Transform2d rightBranchTransform = new Transform2d(0.0, Units.inchesToMeters(8), Rotation2d.kZero);
 
     public enum ReefFace {
         // IMPORTANT: Fudge factors are always positive and should be in meters (use the Units.inchesToMeters() method)
 
         // Blue Reef
-        BLU_REEF_AB(18, 3.657600, 4.025900, 180.0, null, null),
+        BLU_REEF_AB(18, 3.657600 - Units.inchesToMeters(6.25), 4.025900, 180.0, null, null),
         BLU_REEF_CD(17, 4.073906, 3.306318, 240.0, null, null),
         BLU_REEF_EF(22, 4.904740, 3.306318, 300.0, null, null),
         BLU_REEF_GH(21, 5.321046, 4.025900, 0.0, null, null),
diff --git a/src/main/java/frc/robot/vision/commands/DriveToPoseCommand.java b/src/main/java/frc/robot/vision/commands/DriveToPoseCommand.java
index a79b68e..acc1343 100644
--- a/src/main/java/frc/robot/vision/commands/DriveToPoseCommand.java
+++ b/src/main/java/frc/robot/vision/commands/DriveToPoseCommand.java
@@ -3,6 +3,8 @@
 import java.util.List;
 import java.util.function.BooleanSupplier;
 
+import javax.sound.sampled.Line;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -16,6 +18,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.crevolib.math.Conversions;
@@ -26,7 +29,11 @@ public class DriveToPoseCommand extends Command{
     private Command followPathCommand;
     private final Pose2d targetPose;
     CommandSwerveDrivetrain mDrivetrain;
-    public final PathConstraints pathConstraints = new PathConstraints(2, 2, Units.degreesToRadians(360), Units.degreesToRadians(360));
+    public final PathConstraints pathConstraints = new PathConstraints(
+        2, 
+        2, 
+        Units.degreesToRadians(360), 
+        Units.degreesToRadians(360));
     PoseEstimatorSubsystem mPoseEstimatorSubsystem;
 
     public DriveToPoseCommand(Pose2d targetPose) {
@@ -45,9 +52,9 @@ public DriveToPoseCommand(Pose2d targetPose) {
                 ),
                 new PPHolonomicDriveController(
                     // PID constants for translation
-                    new PIDConstants(10, 0.0, 0.0),
+                    new PIDConstants(.025, 0.0, 0.0),
                     // PID constants for rotation
-                    new PIDConstants(6.5, 0, 0.05)
+                    new PIDConstants(.02, 0, 0.05)
                 ),
                 config,
                 // Assume the path needs to be flipped for Red vs Blue, this is normally the case
@@ -88,6 +95,9 @@ public void initialize() {
             followPathCommand = Commands.none();
         }
         followPathCommand.schedule();
+        SmartDashboard.putString("current pose lineup", currentPose.toString());
+        SmartDashboard.putString("target pose lineup", targetPose.toString());
+        SmartDashboard.putString("starting waypoint lineup", startingWaypoint.toString());
     }
 
     public BooleanSupplier atGoal() {
@@ -101,7 +111,7 @@ public void execute(){
     
     @Override
     public boolean isFinished() {
-        return followPathCommand.isFinished();
+        return atGoal().getAsBoolean();
     }
     
     @Override