Skip to content

Ls shooting momentum adjust #116

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Levercpu wants to merge 11 commits into
base: main
Choose a base branch
from
Open

Ls shooting momentum adjust #116

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
8da131d
fixed
Levercpu Mar 14, 2026
2088861
more changes
Levercpu Mar 15, 2026
59c2d1b
more changes
Levercpu Mar 15, 2026
6ba16dd
more changes
Levercpu Mar 15, 2026
713f8b2
all done
Levercpu Mar 16, 2026
b49f0a0
fixed
Levercpu Mar 16, 2026
525c2e5
fixed
Levercpu Mar 17, 2026
c4298ab
more fixes
Levercpu Mar 17, 2026
0415a79
Merge branch 'main' of https://github.com/FRC4048/Java_2026 into LS_M…
Levercpu Mar 18, 2026
32f4978
NEED TO DELETE LOGGING LATER
Levercpu Mar 18, 2026
c39dc05
NEED TO DELETE LOGGING LATER
Levercpu Mar 18, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}
}
Expand Down
19 changes: 8 additions & 11 deletions src/main/java/frc/robot/constants/GameConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -36,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
Expand Down Expand Up @@ -133,13 +131,11 @@ 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 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(.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;
public static final int SHIFT_2_START = 105;
Expand All @@ -160,4 +156,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;

}
117 changes: 86 additions & 31 deletions src/main/java/frc/robot/subsystems/ControllerSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,15 +1,17 @@
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;
import org.dyn4j.UnitConversion;
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;
Expand All @@ -22,10 +24,9 @@ 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";
private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation";
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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));
}
}

Expand Down Expand Up @@ -183,42 +188,92 @@ private void useShotTargets(ShotTargets shotTargets) {
driverEnabled);
}

private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) {
double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose);
double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile);
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);
Copy link
Contributor

@batchen1 batchen1 Mar 16, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@Levercpu Levercpu Mar 16, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed

Logger.recordOutput("adjustedAimPoint", adjustedProfile.targetPose);
double computedDistanceMeters = calculateDistanceMeters(robotPose, adjustedProfile.targetPose);
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);
}

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) {
if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters;
private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) {
if (shootHub) {
return
0.169 * distance * distance
- 1.73 * distance
1.82 * computedDistanceMeters * computedDistanceMeters
- 5.68 * computedDistanceMeters
+ 20.4;
}
return profile.defaultAnglerAngleDegrees;
}

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


// 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, 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.
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);
}
Copy link
Contributor

@batchen1 batchen1 Mar 16, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@Levercpu Levercpu Mar 16, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed

// Linear regression through 3 static shot distance vs time points.
private double calculateFlightTime(double computedDistanceMeters) {
return 0.208*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) {
Copy link
Contributor

@batchen1 batchen1 Mar 16, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@Levercpu Levercpu Mar 16, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed

Copy link
Contributor

@batchen1 batchen1 Mar 17, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

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, boolean shootHub) {
if (shootHub) {
return
91 * computedDistanceMeters * computedDistanceMeters
- 776 * computedDistanceMeters
- 1_380;
}
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)));
return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose, profile.targetPose, DriverStation.getAlliance().get()== DriverStation.Alliance.Blue)));
}

//Getters for all the subsystems to set posistion.
Expand Down Expand Up @@ -282,7 +337,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;
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
77 changes: 0 additions & 77 deletions src/main/java/frc/robot/utils/math/LaunchCalculations.java
View file
Open in desktop

This file was deleted.

Loading