Ls shooting momentum adjust by Levercpu · Pull Request #116 · FRC4048/Java_2026

Levercpu · 2026-03-15T20:50:19Z

Adjust the target position for flight time and robot velocities (and angular velocity too)

batchen1

overall the math looks fine, but the devil is in the details. I'm not sure how to test this. Of course if you deploy and get perfect shots we declare victory, but what if not? do you have a plan how to diagnose any problems?

batchen1 · 2026-03-16T03:11:06Z

src/main/java/frc/robot/constants/GameConstants.java

-
+    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());


these values were just placeholders, if you removed the comment, please measure and plug in the correct valued.

batchen1 · 2026-03-16T03:12:39Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

+        Twist2d momentumAdjustment = getMomentumAdjustment(robotPose,profile.targetPose, 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);


you should change all the calls to use your adjustedProfile instead of profile.

batchen1 · 2026-03-16T03:13:16Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

+        return 0.0633*computedDistanceMeters + 0.647;
+    }
+
+    private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {


how was this formula derived? you need to document it.

1 - I'm not convinced that the flight-time should depend on the speed of the robot at all. I think it is ok to calculate the flight time based only on the distance from the hub. I doubt that considering the speed our robot will be moving this will make a big difference. There are many things that affect the trajectory that we are not considering (physics related such as friction, drag, texture/compression of the fuel will be changing etc.) and this flight-time based only on distance is one that I'm willing to compromise. It will simplify testing by a lot.

2 - regardless - I'm not even sure this code is correct. What you want is the robot's velocity projected on the robot-to-hub directions. Is this what this expression is using? have you tried it with a few cases?

batchen1 · 2026-03-16T03:15:43Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

    }

+    public Twist2d getMomentumAdjustment(Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds, double timeOfFlight) {
+        Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation();


I think the angular velocity due to the offset from the middle of the robot complicates the code and not sure it will affect the shot that much, it may be easier to test without it. Can you add some option to remove it from the calculation?

batchen1 · 2026-03-16T17:17:49Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

+        ChassisSpeeds adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus
+                (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(),angularVelocityAdjustment.getX(),0.0,robotPose.getRotation()));
+        return adjustSpeeds.toTwist2d(-timeOfFlight);
+    }


Here's a suggestion for documenting this code:

Suggested change

}

// 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);

}

Levercpu · 2026-03-17T00:42:34Z

overall the math looks fine, but the devil is in the details. I'm not sure how to test this. Of course if you deploy and get perfect shots we declare victory, but what if not? do you have a plan how to diagnose any problems?

My idea is to do a Logger.recordOutput(adjustProfile.targetPose) and then look at how it changes on advantage scope as you drive around and see if it makes sense. That should (hopefully) be enough before we start testing. The one problem is that hardware seems to be unconvinced with the mathematical/physics equations I implemented. I also need Jackson's true position/delayed pose estimate adjustment branch merged in.

batchen1 · 2026-03-17T01:16:45Z

I think your math is sound and this is the way to go. If hardware is skeptical of the math/physics, just add an option to disable the adjustment completely then they can shoot while not moving.
I'm not sure why you need Jackson's branch, can you explain?
I will review your changed later tonight, but I don't want it merged to main until it is tested. If you can test in simulation and compare the adjusted Angle to the unadjusted angle that should at least show if there are any major issues with the math.

batchen1 · 2026-03-17T14:40:03Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

    }

    private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {


this condition will now always evaluate to false, the adjusted profile is a new object and the == will always fail. Just pass the color as an additional parm.

batchen1 · 2026-03-17T14:41:17Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

+                .getTranslation().getAngle()).vxMetersPerSecond+1);
+    }
    private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {


same comment as the calculateAnglerAngle.

batchen1 · 2026-03-17T14:57:37Z

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

+        return 0.0633*computedDistanceMeters + 0.647;
+    }
+
+    private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {


1 - I'm not convinced that the flight-time should depend on the speed of the robot at all. I think it is ok to calculate the flight time based only on the distance from the hub. I doubt that considering the speed our robot will be moving this will make a big difference. There are many things that affect the trajectory that we are not considering (physics related such as friction, drag, texture/compression of the fuel will be changing etc.) and this flight-time based only on distance is one that I'm willing to compromise. It will simplify testing by a lot.

2 - regardless - I'm not even sure this code is correct. What you want is the robot's velocity projected on the robot-to-hub directions. Is this what this expression is using? have you tried it with a few cases?

…omentum_Adjust

Levercpu · 2026-03-18T02:53:18Z

Response to last comment.

Well I more meant since we are pretending we are in a static problem where we are aiming towards a setPoint d-V_R*t away instead of d, thus since the setPoint varies with time of flight, and the distance to this "fake" static point varies with the velocity of the robot, the time of flight will depend on the robot velocity. At least thats my understanding might be wrong.
It was wrong earlier but I fixed it in latest update
Have to remove the random test logging I added.

Levercpu added 3 commits March 14, 2026 14:42

fixed

8da131d

more changes

2088861

more changes

59c2d1b

Levercpu changed the title ~~Ls momentum adjust~~ Ls shooting momentum adjust Mar 15, 2026

Levercpu added 2 commits March 15, 2026 17:57

more changes

6ba16dd

all done

713f8b2

Levercpu marked this pull request as ready for review March 16, 2026 02:11

Levercpu requested review from MikeKantrowitz, armadilloz and batchen1 as code owners March 16, 2026 02:11

batchen1 requested changes Mar 16, 2026

View reviewed changes

batchen1 reviewed Mar 16, 2026

View reviewed changes

Levercpu added 3 commits March 16, 2026 19:44

fixed

b49f0a0

fixed

525c2e5

more fixes

c4298ab

Levercpu requested a review from batchen1 March 17, 2026 00:42

batchen1 requested changes Mar 17, 2026

View reviewed changes

Levercpu added 2 commits March 17, 2026 21:36

Merge branch 'main' of https://github.com/FRC4048/Java_2026 into LS_M…

0415a79

…omentum_Adjust

NEED TO DELETE LOGGING LATER

32f4978

Levercpu requested a review from batchen1 March 18, 2026 02:53

NEED TO DELETE LOGGING LATER

c39dc05

-    }
+    // 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);
+    }

Conversation

Levercpu commented Mar 15, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

batchen1 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Levercpu commented Mar 17, 2026

Uh oh!

batchen1 commented Mar 17, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Levercpu commented Mar 18, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Levercpu commented Mar 15, 2026 •

edited

Loading