From 8da131d07194f12c2d81803d9dca5f102e9f2556 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Sat, 14 Mar 2026 14:42:48 -0400
Subject: [PATCH 01/10] fixed

---
 .../frc/robot/constants/GameConstants.java     |  3 ++-
 .../robot/utils/math/LaunchCalculations.java   | 18 ++++++++++++++++++
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java
index 13636392..372f940f 100644
--- a/src/main/java/frc/robot/constants/GameConstants.java
+++ b/src/main/java/frc/robot/constants/GameConstants.java
@@ -149,7 +149,7 @@ public enum Mode {
     public static final double RED_HUB_Y_POSITION = 4.0345;
     public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
     public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
-
+    public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = Math.sqrt(Math.pow(X_DISTANCE_BETWEEN_ROBOT_AND_TURRET,2)+Math.pow(Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET,2));
     // Shift timings
     public static final int SHIFT_1_START = 130;
     public static final int SHIFT_2_START = 105;
@@ -170,4 +170,5 @@ public enum Mode {
     public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later
     public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later
     public static final double MAX_VISION_DISTANCE_SIMULATION = 6;
+
 }
diff --git a/src/main/java/frc/robot/utils/math/LaunchCalculations.java b/src/main/java/frc/robot/utils/math/LaunchCalculations.java
index 5ada137a..34ea38eb 100644
--- a/src/main/java/frc/robot/utils/math/LaunchCalculations.java
+++ b/src/main/java/frc/robot/utils/math/LaunchCalculations.java
@@ -1,5 +1,14 @@
 package frc.robot.utils.math;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import frc.robot.constants.Constants;
 import frc.robot.constants.GameConstants;
 
 public class LaunchCalculations {
@@ -73,5 +82,14 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY
         return shooterVelocity;
 
     }
+    public Pose2d adjustTargetPositionForMomentum_Relative (Pose2d target, Vector<N2> relativeRobotVelocities, double timeOfFlight) {
+        Translation2d delta = new Translation2d(relativeRobotVelocities.times(timeOfFlight));
+        return target.transformBy(new Transform2d(delta,new Rotation2d()));
+    }
+
+    public Vector<N2> relativeRobotVelocities(Vector<N2> absoluteRobotVelocities, Pose2d robotPose, Pose2d anchorPoint) {
+        Transform2d transformation = new Transform2d(anchorPoint,robotPose);
+        return new Vector(transformation.toMatrix().times(new Matrix(absoluteRobotVelocities)));
+    }
 
 }

From 208886156abaa567cf140b30be3f279b566a2e95 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Sun, 15 Mar 2026 16:49:32 -0400
Subject: [PATCH 02/10] more changes

---
 .../robot/subsystems/ControllerSubsystem.java | 40 ++++++++++++++-----
 .../robot/utils/math/LaunchCalculations.java  |  8 ----
 2 files changed, 29 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 8574fe51..62b412aa 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -1,6 +1,10 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Robot;
 import frc.robot.utils.math.TurretCalculations;
@@ -8,8 +12,6 @@
 import org.littletonrobotics.junction.Logger;
 
 import frc.robot.RobotContainer;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -25,7 +27,6 @@ public class ControllerSubsystem extends SubsystemBase {
     private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero);
     private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, Constants.RED_HUB_Y_POSITION, Rotation2d.kZero);
     private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero);
-
     private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX";
     private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY";
     private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation";
@@ -77,10 +78,11 @@ public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContai
     @Override
     public void periodic() {
         Pose2d robotPose = getRobotPose();
+        ChassisSpeeds robotSpeeds = getRobotSpeeds();
         ShootState currentState = getCurrentShootState();
 
         updateStopDelayState(currentState);
-        updateTargets(currentState, robotPose);
+        updateTargets(currentState, robotPose, robotSpeeds);
         if(Constants.DEBUG){
             SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString());
             SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters);
@@ -113,6 +115,9 @@ private Pose2d getRobotPose() {
         }
         return drivebase.getPose();
     }
+    private ChassisSpeeds getRobotSpeeds() {
+        return drivebase.getFieldVelocity();
+    }
 
     private boolean shouldUseManualPose() {
         // This can be confusing in case you're on the robot and have disabled the drivetrain
@@ -134,7 +139,7 @@ private void updateStopDelayState(ShootState currentState) {
         }
     }
 
-    private void updateTargets(ShootState state, Pose2d robotPose) {
+    private void updateTargets(ShootState state, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
         switch (state) {
             case STOPPED -> updateStoppedTargets();
             case FIXED -> useShotTargets(FIXED_TARGETS);
@@ -144,16 +149,16 @@ private void updateTargets(ShootState state, Pose2d robotPose) {
                     // No color, do nothing...
                     useShotTargets(FIXED_TARGETS);
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) {
-                    useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPose));
+                    useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPose, robotSpeeds));
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) {
-                    useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose));
+                    useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose, robotSpeeds));
                 } else {
                     // Unknown color, do nothing...
                     useShotTargets(FIXED_TARGETS);
                 }
             }
 
-            case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose));
+            case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose, robotSpeeds));
         }
     }
 
@@ -183,7 +188,9 @@ private void useShotTargets(ShotTargets shotTargets) {
                 driverEnabled);
     }
 
-    private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) {
+    private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
+        profile.targetPose = adjustTargetPositionForMomentum(robotPose,profile.targetPose, robotSpeeds,
+                equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotSpeeds));
         double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose);
         double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
         double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
@@ -205,7 +212,17 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo
 		}
         return profile.defaultAnglerAngleDegrees;
     }
+    public Pose2d adjustTargetPositionForMomentum (Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
+        ChassisSpeeds targetRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(drivebase.getFieldVelocity(), target.relativeTo(robotPose).getTranslation().getAngle());
+        return target.exp(targetRelativeSpeeds.toTwist2d(timeOfFlight));
+    }
+    private double calculateFlightTime(double computedDistanceMeters) {  // TODO: Change later
+        return 0;
+    }
 
+    private double equalizeFlightTime(double initialDistanceMeters, ChassisSpeeds robotVelocity) {
+        return 0;
+    }
     private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
         if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
             double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters;
@@ -217,6 +234,7 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr
         return profile.defaultShooterVelocityRpm;
     }
 
+
     private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) {
         return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), DriverStation.getAlliance().get()== DriverStation.Alliance.Blue)));
     }
@@ -282,7 +300,7 @@ private ShotTargets(
 
     //Class to save all the target poses and each subsystem position at that point so we can calculate true values later on
     private static final class PoseControlProfile {
-        private final Pose2d targetPose;
+        private Pose2d targetPose;
         private final double defaultAnglerAngleDegrees;
         private final double defaultShooterVelocityRpm;
         private final double defaultTurretAngleDegrees;
diff --git a/src/main/java/frc/robot/utils/math/LaunchCalculations.java b/src/main/java/frc/robot/utils/math/LaunchCalculations.java
index 34ea38eb..5cf79a67 100644
--- a/src/main/java/frc/robot/utils/math/LaunchCalculations.java
+++ b/src/main/java/frc/robot/utils/math/LaunchCalculations.java
@@ -82,14 +82,6 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY
         return shooterVelocity;
 
     }
-    public Pose2d adjustTargetPositionForMomentum_Relative (Pose2d target, Vector<N2> relativeRobotVelocities, double timeOfFlight) {
-        Translation2d delta = new Translation2d(relativeRobotVelocities.times(timeOfFlight));
-        return target.transformBy(new Transform2d(delta,new Rotation2d()));
-    }
 
-    public Vector<N2> relativeRobotVelocities(Vector<N2> absoluteRobotVelocities, Pose2d robotPose, Pose2d anchorPoint) {
-        Transform2d transformation = new Transform2d(anchorPoint,robotPose);
-        return new Vector(transformation.toMatrix().times(new Matrix(absoluteRobotVelocities)));
-    }
 
 }

From 59c2d1b7b72720f1acaa2f0e0b4601fcbc408efb Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Sun, 15 Mar 2026 16:50:03 -0400
Subject: [PATCH 03/10] more changes

---
 src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 62b412aa..0308e3a6 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -216,11 +216,11 @@ public Pose2d adjustTargetPositionForMomentum (Pose2d robotPose, Pose2d target,
         ChassisSpeeds targetRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(drivebase.getFieldVelocity(), target.relativeTo(robotPose).getTranslation().getAngle());
         return target.exp(targetRelativeSpeeds.toTwist2d(timeOfFlight));
     }
-    private double calculateFlightTime(double computedDistanceMeters) {  // TODO: Change later
+    private double calculateFlightTime(double computedDistanceMeters) { // TODO: Change later
         return 0;
     }
 
-    private double equalizeFlightTime(double initialDistanceMeters, ChassisSpeeds robotVelocity) {
+    private double equalizeFlightTime(double initialDistanceMeters, ChassisSpeeds robotVelocity) { // TODO: Change later
         return 0;
     }
     private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {

From 6ba16dd63d47ee21995166d55fbca81fe30d5444 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Sun, 15 Mar 2026 17:57:09 -0400
Subject: [PATCH 04/10] more changes

---
 .../frc/robot/constants/GameConstants.java    | 15 ++--
 .../robot/subsystems/ControllerSubsystem.java | 16 ++--
 .../robot/utils/math/LaunchCalculations.java  | 87 -------------------
 .../robot/utils/math/TurretCalculations.java  | 35 +++-----
 4 files changed, 24 insertions(+), 129 deletions(-)
 delete mode 100644 src/main/java/frc/robot/utils/math/LaunchCalculations.java

diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java
index 372f940f..b8307ea2 100644
--- a/src/main/java/frc/robot/constants/GameConstants.java
+++ b/src/main/java/frc/robot/constants/GameConstants.java
@@ -1,8 +1,6 @@
 package frc.robot.constants;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -143,13 +141,10 @@ public enum Mode {
     public static final double GRAVITY = 9.81;
     public static final double HUB_HEIGHT = 1.83;
     public static final double SHOOTER_HEIGHT = 0.5;
-    public static final double BLUE_HUB_X_POSITION = 4.6256;
-    public static final double BLUE_HUB_Y_POSITION = 4.0345;
-    public static final double RED_HUB_X_POSITION = 11.9154;
-    public static final double RED_HUB_Y_POSITION = 4.0345;
-    public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
-    public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
-    public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = Math.sqrt(Math.pow(X_DISTANCE_BETWEEN_ROBOT_AND_TURRET,2)+Math.pow(Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET,2));
+    public static final Pose2d BLUE_HUB_POS = new Pose2d(4.6256, 4.0345, new Rotation2d());
+    public static final Pose2d RED_HUB_POS = new Pose2d(11.9154, 4.0345, new Rotation2d());
+    public static final Transform2d TURRET_OFFSET = new Transform2d(.4, .4, new Rotation2d());
+    public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = TURRET_OFFSET.getTranslation().getNorm();
     // Shift timings
     public static final int SHIFT_1_START = 130;
     public static final int SHIFT_2_START = 105;
diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 0308e3a6..88923053 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -24,8 +24,8 @@ public class ControllerSubsystem extends SubsystemBase {
     private static final double STOP_DELAY_SECONDS = 0.5;
 
     // Placeholder target poses until real field target values are finalized
-    private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero);
-    private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, Constants.RED_HUB_Y_POSITION, Rotation2d.kZero);
+    private static final Pose2d BLUE_HUB_TARGET_POSE = Constants.BLUE_HUB_POS;
+    private static final Pose2d RED_HUB_TARGET_POSE = Constants.RED_HUB_POS;
     private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero);
     private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX";
     private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY";
@@ -189,8 +189,9 @@ private void useShotTargets(ShotTargets shotTargets) {
     }
 
     private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
-        profile.targetPose = adjustTargetPositionForMomentum(robotPose,profile.targetPose, robotSpeeds,
+        Twist2d momentumAdjustment = getMomentumAdjustment(robotPose,profile.targetPose, robotSpeeds,
                 equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotSpeeds));
+        profile.targetPose.exp(momentumAdjustment);
         double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose);
         double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
         double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
@@ -199,7 +200,7 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d
     }
 
     private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) {
-        return robotPose.getTranslation().getDistance(targetPose.getTranslation());
+        return robotPose.transformBy(Constants.TURRET_OFFSET).getTranslation().getDistance(targetPose.getTranslation());
     }
 
     private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
@@ -212,9 +213,10 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo
 		}
         return profile.defaultAnglerAngleDegrees;
     }
-    public Pose2d adjustTargetPositionForMomentum (Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
+
+    public Twist2d getMomentumAdjustment(Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
         ChassisSpeeds targetRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(drivebase.getFieldVelocity(), target.relativeTo(robotPose).getTranslation().getAngle());
-        return target.exp(targetRelativeSpeeds.toTwist2d(timeOfFlight));
+        return targetRelativeSpeeds.toTwist2d(timeOfFlight);
     }
     private double calculateFlightTime(double computedDistanceMeters) { // TODO: Change later
         return 0;
@@ -236,7 +238,7 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr
 
 
     private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) {
-        return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), DriverStation.getAlliance().get()== DriverStation.Alliance.Blue)));
+        return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose, profile.targetPose, DriverStation.getAlliance().get()== DriverStation.Alliance.Blue)));
     }
 
     //Getters for all the subsystems to set posistion.
diff --git a/src/main/java/frc/robot/utils/math/LaunchCalculations.java b/src/main/java/frc/robot/utils/math/LaunchCalculations.java
deleted file mode 100644
index 5cf79a67..00000000
--- a/src/main/java/frc/robot/utils/math/LaunchCalculations.java
+++ /dev/null
@@ -1,87 +0,0 @@
-package frc.robot.utils.math;
-
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Vector;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N2;
-import frc.robot.constants.Constants;
-import frc.robot.constants.GameConstants;
-
-public class LaunchCalculations {
-
-    /*
-     * The intention of this class is to provide the launch angle and initial velocity given
-     * the robot's distance from the hub. We will test this on the robot, and if the physics
-     * don't work out practically, we will use this class to give initial test values for the
-     * robot. We will run these test values to obtain 4 points experimentally. Then, we will
-     * use interpolation to generate a function that will then give the correct launch angle
-     * and velocity according to our current distance from the robot. 
-     */
-
-    // The origin is defined as the bottom right corner of the blue alliance
-    
-    // All angles are in radians, all distances are in meters, all velocities in m/s
-
-    public static double calculateShooterVelocity(double robotPosX, double robotPosY, double launchAngle, boolean isBlueAlliance) {
-        
-        // Target value -- what we're trying to find
-        double shooterVelocity; // Initial velocity of the shooter -- related to flywheel speed
-
-        // calculates the position of the turret with respect to the origin using the robot center 
-        // and the constant distance between the robot center and the turret.
-        double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-        double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-
-        // Given values -- from Constants file
-        double hubHeight = GameConstants.HUB_HEIGHT; // height of the top of the hub
-        double shooterHeight = GameConstants.SHOOTER_HEIGHT; // height of shooter
-        double hubPosX; 
-        double hubPosY;
-
-        double deltaDistance; // distance between the robot and the hub
-        double deltaHeight; // height difference between shooter and hub
-        
-        if (isBlueAlliance) {
-            // hub position determined by which alliance robot is on
-            hubPosX = GameConstants.BLUE_HUB_X_POSITION;
-            hubPosY = GameConstants.BLUE_HUB_Y_POSITION;
-        }
-        else {
-            hubPosX = GameConstants.RED_HUB_X_POSITION;
-            hubPosY = GameConstants.RED_HUB_Y_POSITION;
-        }
-        
-        // Compute distance between hub and robot with Pythagorean Theorem
-        deltaDistance = Math.sqrt(Math.pow(hubPosY - turretPosY, 2) 
-        + Math.pow(hubPosX - turretPosX, 2));
-
-        // Compute the change in height between the shooter and the hub
-        deltaHeight = hubHeight - shooterHeight;
-
-        /*
-        *  Compute the shooter velocity. It is a physics formula that computes the initial velocity given
-        *  the launch angle, the distance between the robot and the hub, the height change, and
-        *  the gravitational constant g. This formula has been tested in projectile motion simulators.
-        *  It starts with kinematic equations, then you set up a system of two of the kinematic equationsm
-        *  eliminate time, and solve for the initial velocity.
-        */
-        shooterVelocity = Math.sqrt(
-            ( 
-                (GameConstants.GRAVITY) * Math.pow(deltaDistance, 2) * Math.pow(1 / Math.cos(launchAngle), 2)
-            )
-            /
-            2 * (
-                deltaDistance * Math.tan(launchAngle) - deltaHeight
-            )
-        );
-
-        return shooterVelocity;
-
-    }
-
-
-}
diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java
index 7e88c7b0..b493afe6 100644
--- a/src/main/java/frc/robot/utils/math/TurretCalculations.java
+++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java
@@ -1,5 +1,9 @@
 package frc.robot.utils.math;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.robot.constants.GameConstants;
 
 public class TurretCalculations {
@@ -26,31 +30,11 @@ public class TurretCalculations {
     // all positions are in meters
     // robotRotation is in radians
     // Returns - turret angle in radians
-    public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) {
-        
-        // Get turret offset from robot center
-        double turretOffsetX = GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-        double turretOffsetY = GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
+    public static double calculateTurretAngle(Pose2d robotPos,Pose2d adjTargetPose, boolean isBlueAlliance) {
 
-        // Rotate the offset by robot rotation (offset rotates with robot)
-        double rotatedOffsetX = turretOffsetX * Math.cos(robotRotation) - turretOffsetY * Math.sin(robotRotation);
-        double rotatedOffsetY = turretOffsetX * Math.sin(robotRotation) + turretOffsetY * Math.cos(robotRotation);
+        Pose2d turretPos = robotPos.transformBy(GameConstants.TURRET_OFFSET);
 
-        // Calculate actual turret position on field
-        double turretPosX = robotPosX + rotatedOffsetX;
-        double turretPosY = robotPosY + rotatedOffsetY;
-
-        double hubPosX;
-        double hubPosY;
-
-        if (isBlueAlliance) {
-            // hub position determined by which alliance robot is on
-            hubPosX = GameConstants.BLUE_HUB_X_POSITION;
-            hubPosY = GameConstants.BLUE_HUB_Y_POSITION;
-        } else {
-            hubPosX = GameConstants.RED_HUB_X_POSITION;
-            hubPosY = GameConstants.RED_HUB_Y_POSITION;
-        }
+        Pose2d hubPos = adjTargetPose;
 
         /*
          * This finds the unadjusted pan angle (assuming there is no robot rotation) using
@@ -60,7 +44,8 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
          * of the input numbers.
          * 
          */
-        double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX);
+        Translation2d hubRelativeToRobot = hubPos.relativeTo(turretPos).getTranslation();
+        double panAngleUnadjusted = hubRelativeToRobot.getAngle().getRadians();
 
         /*
          * Adjusts the pan angle to account for the robot's current rotation. We subtract the
@@ -68,7 +53,7 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
          * pan angle, which is the proper angle of the turret adjusted for the robot's rotation
          * and the fact that the turret 0 angle in in the back of the robot.
          */
-        double panAngle = panAngleUnadjusted - (robotRotation + Math.PI);
+        double panAngle = panAngleUnadjusted - (robotPos.getRotation().getRadians() + Math.PI);
 
         // normalize angle between -PI and PI
         double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI));

From 713f8b26d244c1b1b5492d4fd93a6e802c237985 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Sun, 15 Mar 2026 22:10:41 -0400
Subject: [PATCH 05/10] all done

---
 .../robot/subsystems/ControllerSubsystem.java | 20 +++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 88923053..d6ff8742 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -190,8 +190,9 @@ private void useShotTargets(ShotTargets shotTargets) {
 
     private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
         Twist2d momentumAdjustment = getMomentumAdjustment(robotPose,profile.targetPose, robotSpeeds,
-                equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotSpeeds));
-        profile.targetPose.exp(momentumAdjustment);
+                equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds));
+        PoseControlProfile adjustedProfile = new PoseControlProfile(profile.targetPose, profile.defaultAnglerAngleDegrees, profile.defaultShooterVelocityRpm, profile.defaultTurretAngleDegrees);
+        adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment);
         double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose);
         double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
         double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
@@ -215,15 +216,18 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo
     }
 
     public Twist2d getMomentumAdjustment(Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
-        ChassisSpeeds targetRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(drivebase.getFieldVelocity(), target.relativeTo(robotPose).getTranslation().getAngle());
-        return targetRelativeSpeeds.toTwist2d(timeOfFlight);
+        Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation();
+        ChassisSpeeds adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus
+                (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(),angularVelocityAdjustment.getX(),0.0,robotPose.getRotation()));
+        return adjustSpeeds.toTwist2d(-timeOfFlight);
     }
-    private double calculateFlightTime(double computedDistanceMeters) { // TODO: Change later
-        return 0;
+    private double calculateFlightTime(double computedDistanceMeters) {
+        return 0.0633*computedDistanceMeters + 0.647;
     }
 
-    private double equalizeFlightTime(double initialDistanceMeters, ChassisSpeeds robotVelocity) { // TODO: Change later
-        return 0;
+    private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {
+        return (633*initialDistanceMeters+6470)/(633*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.relativeTo(robotPose)
+                .getTranslation().getAngle()).vxMetersPerSecond+10000);
     }
     private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
         if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {

From b49f0a0d9427e2d7652614f3496e99c2fdb44402 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Mon, 16 Mar 2026 19:44:05 -0400
Subject: [PATCH 06/10] fixed

---
 .../robot/subsystems/ControllerSubsystem.java  | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index d6ff8742..214af6ce 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -215,12 +215,30 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo
         return profile.defaultAnglerAngleDegrees;
     }
 
+
+
+    // Twist2d is a change in pose (position and rotation) over time.
+    // the returned value is a field-relative displacement vector (change in position and rotation) over the ball flight time.
+    // this means how much we need to adjust our shot.
     public Twist2d getMomentumAdjustment(Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
+        // calculate the change per second of the turret's position relative to the center due to robot rotation. This
+        // is basically angular speed. Result is in robot coordinate system.
         Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation();
+        
+        // take the robot's current speed (relative to field)
+        // convert the turret's angular velocity from robot-relative to field-relative
+        // since the rotation velocity is perpendicular to the robot x-axis, so we need to rotate it by 90 degrees
+        // which is why we use -Y,X instead of X,Y for the conversion.
+        // add the two to get the effective speed of the turret/projectile.
         ChassisSpeeds adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus
                 (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(),angularVelocityAdjustment.getX(),0.0,robotPose.getRotation()));
+        
+        // convert the effective speed to a twist (displacement over time)
+        // positive timeOfFlight tells us how much we move
+        // negative timeOfFlight tells us how much we need to move to compensate for our movement
         return adjustSpeeds.toTwist2d(-timeOfFlight);
     }
+
     private double calculateFlightTime(double computedDistanceMeters) {
         return 0.0633*computedDistanceMeters + 0.647;
     }

From 525c2e57a71a83f2ae9db0cd3ccda7c1a2646716 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Mon, 16 Mar 2026 20:19:47 -0400
Subject: [PATCH 07/10] fixed

---
 .../frc/robot/constants/GameConstants.java    |  3 +-
 .../robot/subsystems/ControllerSubsystem.java | 47 +++++++++++--------
 2 files changed, 30 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java
index b8307ea2..bef76af7 100644
--- a/src/main/java/frc/robot/constants/GameConstants.java
+++ b/src/main/java/frc/robot/constants/GameConstants.java
@@ -143,7 +143,8 @@ public enum Mode {
     public static final double SHOOTER_HEIGHT = 0.5;
     public static final Pose2d BLUE_HUB_POS = new Pose2d(4.6256, 4.0345, new Rotation2d());
     public static final Pose2d RED_HUB_POS = new Pose2d(11.9154, 4.0345, new Rotation2d());
-    public static final Transform2d TURRET_OFFSET = new Transform2d(.4, .4, new Rotation2d());
+    public static final Transform2d TURRET_OFFSET = new Transform2d(.4, .4, new Rotation2d()); //TODO: needs value from hardware
+    public static final boolean ACCOUNT_FOR_ANGULAR_MOMENTUM = false;
     public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = TURRET_OFFSET.getTranslation().getNorm();
     // Shift timings
     public static final int SHIFT_1_START = 130;
diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 214af6ce..5e0b4603 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -189,14 +189,14 @@ private void useShotTargets(ShotTargets shotTargets) {
     }
 
     private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
-        Twist2d momentumAdjustment = getMomentumAdjustment(robotPose,profile.targetPose, robotSpeeds,
+        Twist2d momentumAdjustment = getMomentumAdjustment(robotPose, Constants.ACCOUNT_FOR_ANGULAR_MOMENTUM, robotSpeeds,
                 equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds));
         PoseControlProfile adjustedProfile = new PoseControlProfile(profile.targetPose, profile.defaultAnglerAngleDegrees, profile.defaultShooterVelocityRpm, profile.defaultTurretAngleDegrees);
         adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment);
-        double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose);
-        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
-        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
-        double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile);
+        double computedDistanceMeters = calculateDistanceMeters(robotPose, adjustedProfile.targetPose);
+        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, adjustedProfile);
+        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, adjustedProfile);
+        double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, adjustedProfile);
         return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true);
     }
 
@@ -220,32 +220,41 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo
     // Twist2d is a change in pose (position and rotation) over time.
     // the returned value is a field-relative displacement vector (change in position and rotation) over the ball flight time.
     // this means how much we need to adjust our shot.
-    public Twist2d getMomentumAdjustment(Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
+    public Twist2d getMomentumAdjustment(Pose2d robotPose, boolean useAngularMomentumAdjustment, ChassisSpeeds robotSpeeds, double timeOfFlight) {
         // calculate the change per second of the turret's position relative to the center due to robot rotation. This
         // is basically angular speed. Result is in robot coordinate system.
-        Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation();
-        
-        // take the robot's current speed (relative to field)
-        // convert the turret's angular velocity from robot-relative to field-relative
-        // since the rotation velocity is perpendicular to the robot x-axis, so we need to rotate it by 90 degrees
-        // which is why we use -Y,X instead of X,Y for the conversion.
-        // add the two to get the effective speed of the turret/projectile.
-        ChassisSpeeds adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus
-                (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(),angularVelocityAdjustment.getX(),0.0,robotPose.getRotation()));
-        
+        ChassisSpeeds adjustSpeeds;
+        if (useAngularMomentumAdjustment) {
+            Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation();
+
+
+            // take the robot's current speed (relative to field)
+            // convert the turret's angular velocity from robot-relative to field-relative
+            // since the rotation velocity is perpendicular to the robot x-axis, so we need to rotate it by 90 degrees
+            // which is why we use -Y,X instead of X,Y for the conversion.
+            // add the two to get the effective speed of the turret/projectile.
+            adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus
+                    (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(), angularVelocityAdjustment.getX(), 0.0, robotPose.getRotation()));
+        } else {
+            adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0));
+        }
         // convert the effective speed to a twist (displacement over time)
         // positive timeOfFlight tells us how much we move
         // negative timeOfFlight tells us how much we need to move to compensate for our movement
         return adjustSpeeds.toTwist2d(-timeOfFlight);
     }
-
+    // Linear regression through 3 static shot distance vs time points.
     private double calculateFlightTime(double computedDistanceMeters) {
         return 0.0633*computedDistanceMeters + 0.647;
     }
 
+    // Since t(x)=mx+b (previous function), we can solve for x
+    // t = m*(d - v_robot * t)+b
+    // t + v_robot * t = m * d + b
+    // t = (m * d + b) / (v_robot + 1)
     private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {
-        return (633*initialDistanceMeters+6470)/(633*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.relativeTo(robotPose)
-                .getTranslation().getAngle()).vxMetersPerSecond+10000);
+        return (0.0633*initialDistanceMeters+0.647)/(0.0633*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.relativeTo(robotPose)
+                .getTranslation().getAngle()).vxMetersPerSecond+1);
     }
     private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
         if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {

From c4298abf3c41d6d955d7936d5215d8041b72a0fa Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Mon, 16 Mar 2026 20:42:14 -0400
Subject: [PATCH 08/10] more fixes

---
 .../robot/subsystems/ControllerSubsystem.java  | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 5e0b4603..62fbcbbf 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -206,10 +206,9 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) {
 
     private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
         if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
-            double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters;
 			return
-				0.169 * distance * distance
-			  - 1.73 * distance
+				1.82 * computedDistanceMeters * computedDistanceMeters
+			  - 5.68 * computedDistanceMeters
 	          + 20.4;
 		}
         return profile.defaultAnglerAngleDegrees;
@@ -245,7 +244,7 @@ public Twist2d getMomentumAdjustment(Pose2d robotPose, boolean useAngularMomentu
     }
     // Linear regression through 3 static shot distance vs time points.
     private double calculateFlightTime(double computedDistanceMeters) {
-        return 0.0633*computedDistanceMeters + 0.647;
+        return 0.208*computedDistanceMeters + 0.647;
     }
 
     // Since t(x)=mx+b (previous function), we can solve for x
@@ -253,16 +252,15 @@ private double calculateFlightTime(double computedDistanceMeters) {
     // t + v_robot * t = m * d + b
     // t = (m * d + b) / (v_robot + 1)
     private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {
-        return (0.0633*initialDistanceMeters+0.647)/(0.0633*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.relativeTo(robotPose)
+        return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.relativeTo(robotPose)
                 .getTranslation().getAngle()).vxMetersPerSecond+1);
     }
     private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
         if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
-            double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters;
-			return Math.floor(
-				8.46 * distance * distance
-			  - 237 * distance
-			  - 1_380);
+			return
+				91 * computedDistanceMeters * computedDistanceMeters
+			  - 776 * computedDistanceMeters
+			  - 1_380;
 		}
         return profile.defaultShooterVelocityRpm;
     }

From 32f4978759eecfb55ec6a59678a05dd74f7753a1 Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Tue, 17 Mar 2026 22:40:42 -0400
Subject: [PATCH 09/10] NEED TO DELETE LOGGING LATER

---
 src/main/java/frc/robot/Robot.java                       | 2 +-
 src/main/java/frc/robot/constants/GameConstants.java     | 6 +++---
 .../java/frc/robot/subsystems/ControllerSubsystem.java   | 9 +++++++--
 .../robot/subsystems/swervedrive/SwerveSubsystem.java    | 1 +
 4 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index e3eca60f..3211a41e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -142,7 +142,7 @@ public void robotPeriodic() {
                 SmartDashboard.putBoolean("Hub Active?", hubActive());
                 if (Constants.currentMode == Constants.Mode.SIM) {
                     Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get());
-                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationPose().get());
+                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationRawOdomPose());
                 }
             }
         }
diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java
index 3143a89a..f4662b13 100644
--- a/src/main/java/frc/robot/constants/GameConstants.java
+++ b/src/main/java/frc/robot/constants/GameConstants.java
@@ -34,7 +34,7 @@ public enum Mode {
     public static final boolean ENABLE_LOGGING = true;
 
     //Debugs
-    public static final boolean DEBUG = false;
+    public static final boolean DEBUG = true;
     public static final boolean ARM_DEBUG = true;
 
     //Joystick
@@ -133,8 +133,8 @@ public enum Mode {
     public static final double SHOOTER_HEIGHT = 0.5;
     public static final Pose2d BLUE_HUB_POS = new Pose2d(4.6256, 4.0345, new Rotation2d());
     public static final Pose2d RED_HUB_POS = new Pose2d(11.9154, 4.0345, new Rotation2d());
-    public static final Transform2d TURRET_OFFSET = new Transform2d(.4, .4, new Rotation2d()); //TODO: needs value from hardware
-    public static final boolean ACCOUNT_FOR_ANGULAR_MOMENTUM = false;
+    public static final Transform2d TURRET_OFFSET = new Transform2d(.15, 0, new Rotation2d()); //TODO: needs value from hardware
+    public static final boolean ACCOUNT_FOR_ANGULAR_MOMENTUM = true;
     public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = TURRET_OFFSET.getTranslation().getNorm();
     // Shift timings
     public static final int SHIFT_1_START = 130;
diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 62fbcbbf..5503db93 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -189,10 +189,13 @@ private void useShotTargets(ShotTargets shotTargets) {
     }
 
     private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
+        Logger.recordOutput("equalizedTime",equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds));
+        Logger.recordOutput("nonEqualizedTime", calculateFlightTime(calculateDistanceMeters(robotPose, profile.targetPose)));
         Twist2d momentumAdjustment = getMomentumAdjustment(robotPose, Constants.ACCOUNT_FOR_ANGULAR_MOMENTUM, robotSpeeds,
                 equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds));
         PoseControlProfile adjustedProfile = new PoseControlProfile(profile.targetPose, profile.defaultAnglerAngleDegrees, profile.defaultShooterVelocityRpm, profile.defaultTurretAngleDegrees);
         adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment);
+        Logger.recordOutput("adjustedAimPoint", adjustedProfile.targetPose);
         double computedDistanceMeters = calculateDistanceMeters(robotPose, adjustedProfile.targetPose);
         double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, adjustedProfile);
         double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, adjustedProfile);
@@ -252,8 +255,10 @@ private double calculateFlightTime(double computedDistanceMeters) {
     // t + v_robot * t = m * d + b
     // t = (m * d + b) / (v_robot + 1)
     private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {
-        return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.relativeTo(robotPose)
-                .getTranslation().getAngle()).vxMetersPerSecond+1);
+        Logger.recordOutput("robot to target translation", target.getTranslation().minus(robotPose.getTranslation()));
+        Logger.recordOutput("robot to target angle", target.getTranslation().minus(robotPose.getTranslation()).getAngle());
+        Logger.recordOutput("speed Approaching Hub", ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond);
+        return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond+1);
     }
     private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
         if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index ef7becba..4fb69bae 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -124,6 +124,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
     public void periodic() {
         //add vision pose here
         //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d()));
+        Logger.recordOutput("fieldVelocities",getFieldVelocity());
     }
 
     @Override

From c39dc05641d0bbe232552ac5f22f17d2fedc56af Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Tue, 17 Mar 2026 22:53:54 -0400
Subject: [PATCH 10/10] NEED TO DELETE LOGGING LATER

---
 .../frc/robot/subsystems/ControllerSubsystem.java   | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
index 5503db93..07ade16c 100644
--- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java
@@ -197,8 +197,9 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d
         adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment);
         Logger.recordOutput("adjustedAimPoint", adjustedProfile.targetPose);
         double computedDistanceMeters = calculateDistanceMeters(robotPose, adjustedProfile.targetPose);
-        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, adjustedProfile);
-        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, adjustedProfile);
+        boolean shootHub = profile == BLUE_HUB_PROFILE || profile == RED_HUB_PROFILE;
+        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, adjustedProfile, shootHub);
+        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, adjustedProfile, shootHub);
         double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, adjustedProfile);
         return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true);
     }
@@ -207,8 +208,8 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) {
         return robotPose.transformBy(Constants.TURRET_OFFSET).getTranslation().getDistance(targetPose.getTranslation());
     }
 
-    private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
-        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
+    private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) {
+        if (shootHub) {
 			return
 				1.82 * computedDistanceMeters * computedDistanceMeters
 			  - 5.68 * computedDistanceMeters
@@ -260,8 +261,8 @@ private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose
         Logger.recordOutput("speed Approaching Hub", ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond);
         return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond+1);
     }
-    private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
-        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
+    private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) {
+        if (shootHub) {
 			return
 				91 * computedDistanceMeters * computedDistanceMeters
 			  - 776 * computedDistanceMeters