diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ReefBranchAlign.java b/src/main/java/frc/robot/subsystems/drivetrain/ReefBranchAlign.java
index 303f4b9..597ba0f 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/ReefBranchAlign.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/ReefBranchAlign.java
@@ -31,14 +31,17 @@ public class ReefBranchAlign extends Command {
     private SwerveRequest.ApplyRobotSpeeds m_drive;
     private Transform2d m_shift;
     private DoubleSupplier m_joystickInput;
+    private Pose2d m_lastPose;
   
   
     public ReefBranchAlign(CommandSwerveDrivetrain drivetrain, Transform2d shift, DoubleSupplier joystickXInput) {
       m_shift = shift;
+      m_lastPose = new Pose2d();
+
       m_joystickInput = joystickXInput;
 
       m_translationController =
-          new ProfiledPIDController(4.0, 0.0, 0.1, new TrapezoidProfile.Constraints(2, 2));
+          new ProfiledPIDController(4.0, 0.0, 0.1, new TrapezoidProfile.Constraints(7, 7));
       m_thetaController =
           new ProfiledPIDController(
               5.0, 0.0, 0.1, new TrapezoidProfile.Constraints(1 * Math.PI, 1 * Math.PI));
@@ -54,25 +57,16 @@ public ReefBranchAlign(CommandSwerveDrivetrain drivetrain, Transform2d shift, Do
   @Override
   public void initialize() {
     m_goalPose = m_drivetrain.findClosestNode().transformBy(m_shift);
-
     Pose2d initialPose = m_drivetrain.getState().Pose;
+
     m_translationController.setTolerance(0.1);
     m_thetaController.setTolerance(Units.degreesToRadians(3.0));
     m_thetaController.enableContinuousInput(-Math.PI, Math.PI);
     ChassisSpeeds fieldVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(m_drivetrain.getState().Speeds, initialPose.getRotation());
 
     m_translationController.reset(
-        initialPose.getTranslation().getDistance(m_goalPose.getTranslation()),
-        Math.min(
-            0.0,
-            -new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond)
-                .rotateBy(
-                    m_goalPose
-                        .getTranslation()
-                        .minus(initialPose.getTranslation())
-                        .getAngle()
-                        .unaryMinus())
-                .getX()));
+        Math.abs(initialPose.getTranslation().getY() - m_goalPose.getTranslation().getY()),
+        Math.min(0.0, m_drivetrain.getState().Speeds.vyMetersPerSecond));
     m_thetaController.reset(
         initialPose.getRotation().getRadians(),
         fieldVelocity.omegaRadiansPerSecond);
@@ -82,60 +76,55 @@ public void initialize() {
   @Override
   public void execute() {
     Pose2d currentPose = m_drivetrain.getState().Pose;
+    double distanceToGoalPose = currentPose.getTranslation().getY() - m_goalPose.getTranslation().getY();
 
-    double distanceToGoalPose =
-        currentPose.getTranslation().getDistance(m_goalPose.getTranslation());
+    // double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0);
 
-    double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0);
+    // m_translationController.reset(
+    //     Math.abs(m_lastSetpointTranslation.getY() - m_goalPose.getY()),
+    //     m_translationController.getSetpoint().velocity);//ROBO RELATIVE
 
-    m_translationController.reset(
-        m_lastSetpointTranslation.getDistance(m_goalPose.getTranslation()),
-        m_translationController.getSetpoint().velocity);//ROBO RELATIVE
+    double driveVelocity =
+        // m_translationController.getSetpoint().velocity * ffScaler
+            m_translationController.calculate(distanceToGoalPose, 0.0);
 
-    double driveVelocityScalar =
-        m_translationController.getSetpoint().velocity * ffScaler
-            + m_translationController.calculate(distanceToGoalPose, 0.0);
-
-    if (distanceToGoalPose < m_translationController.getPositionTolerance())
-      driveVelocityScalar = 0.0;
+    // if (distanceToGoalPose < m_translationController.getPositionTolerance())
+    //   driveVelocityScalar = 0.0;
       
-    m_lastSetpointTranslation =
-        new Pose2d(
-                m_goalPose.getTranslation(),
-                currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
-            .transformBy(
-                new Transform2d(
-                    new Translation2d(m_translationController.getSetpoint().position, 0.0),
-                    new Rotation2d()))
-            .getTranslation();
-
-    double thetaVelocity =
-        m_thetaController.getSetpoint().velocity * ffScaler
-            + m_thetaController.calculate(
-                currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians());
-    double thetaErrorAbsolute =
-        Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians());
-    if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0;
-
-    Translation2d driveVelocity =
-        new Pose2d(
-                new Translation2d(),
-                currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
-            .transformBy(
-                new Transform2d(new Translation2d(driveVelocityScalar, 0.0), new Rotation2d()))
-            .getTranslation();
-
-    final ChassisSpeeds CS = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(driveVelocity.getX(), driveVelocity.getY(), thetaVelocity), currentPose.getRotation());
-
-    m_drivetrain.setControl(m_drive.withSpeeds(new ChassisSpeeds(m_joystickInput.getAsDouble(), CS.vyMetersPerSecond, CS.omegaRadiansPerSecond)));
+    // m_lastSetpointTranslation = 
+    //     new Pose2d(
+    //             m_goalPose.getTranslation(),
+    //             currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
+    //         .transformBy(
+    //             new Transform2d(
+    //                 new Translation2d(m_translationController.getSetpoint().position, 0.0),
+    //                 new Rotation2d()))
+    //         .getTranslation();
+
+    double thetaVelocity = m_thetaController.calculate(currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians());
+        // m_thetaController.getSetpoint().velocity * ffScaler
+            
+
+    // double thetaErrorAbsolute =
+    //     Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians());
+    // if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0;
+
+    // Translation2d driveVelocity =
+    //     new Pose2d(
+    //             new Translation2d(),
+    //             currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
+    //         .transformBy(
+    //             new Transform2d(new Translation2d(driveVelocityScalar, 0.0), new Rotation2d()))
+    //         .getTranslation();
+
+    final ChassisSpeeds CS = new ChassisSpeeds(m_joystickInput.getAsDouble(), driveVelocity, thetaVelocity);
+
+    m_drivetrain.setControl(m_drive.withSpeeds(CS));
 
     Logger.recordOutput("Drivetrain/DriveToPose/ChassisSpeeds", CS);
-
     Logger.recordOutput("Drivetrain/DriveToPose/DistanceToGoalPose", distanceToGoalPose);
-    Logger.recordOutput(
-        "Drivetrain/DriveToPose/Setpoint",
-        new Pose2d(
-            m_lastSetpointTranslation, new Rotation2d(m_thetaController.getSetpoint().position)));
     Logger.recordOutput("Drivetrain/DriveToPose/Goal", m_goalPose);
+
+    m_lastPose = currentPose;
   }
 }
\ No newline at end of file