Skip to content

lead compensator v1.5 #430

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

Draft
pranavgundu wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

lead compensator v1.5 #430

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
514aaa7
new lead logic
pranavgundu Mar 28, 2026
c11b0b3
Merge branch 'main' into lead-compensator-v1.5
pranavgundu Mar 28, 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
124 changes: 78 additions & 46 deletions src/main/java/frc/robot/Orchestrator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,14 @@
package frc.robot;

import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM;
import static frc.robot.subsystems.shooter.ShooterConstants.*;

import edu.wpi.first.math.filter.LinearFilter;
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.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -22,31 +25,27 @@
import frc.robot.subsystems.intake.rollers.IntakeRollers;
import frc.robot.subsystems.magicCarpet.MagicCarpet;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.util.ShooterLeadCompensator;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;

public class Orchestrator {
private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0);
private static final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0);
private static final double PHASE_DELAY_SECONDS = 0.03;
private static final int CONVERGENCE_ITERATIONS = 20;
private static final double ANGLE_ALPHA = 0.15;

private final Drive drive;
private final Shooter shooter;
private final MagicCarpet magicCarpet;
private final Indexer indexer;
private final Intake intake;
private final IntakeRollers rollers;
private final RobotState robotState = RobotState.getInstance();
private final ShooterLeadCompensator shooterLeadCompensator;
private final CommandXboxController driverController;

// Wraparound-safe angle filter
private double filteredAngleRad = Double.NaN;
private static final double ANGLE_ALPHA =
0.15; // tune: lower = smoother, higher = more responsive

// Filtering for lead compensator outputs
private final LinearFilter targetXFilter = LinearFilter.singlePoleIIR(0.06, 0.02);
private final LinearFilter targetYFilter = LinearFilter.singlePoleIIR(0.06, 0.02);
private final LinearFilter distanceFilter = LinearFilter.singlePoleIIR(0.06, 0.02);

public Orchestrator(
Drive drive,
Expand All @@ -62,27 +61,64 @@ public Orchestrator(
this.indexer = indexer;
this.intake = intake;
this.rollers = rollers;
this.shooterLeadCompensator = new ShooterLeadCompensator(drive, shooter);
this.driverController = driverController;
}

private Translation2d getShooterPosition(Pose2d robotPose) {
Translation2d rotatedOffset =
kShooterOffsetFromRobotCenter.getTranslation().rotateBy(robotPose.getRotation());
return robotPose.getTranslation().plus(rotatedOffset);
}

private record LeadResult(Pose2d target, double distance, double timeOfFlight) {}

private LeadResult computeLeadCompensation(Translation2d targetPosition) {
Pose2d robotPose = drive.getPose();
ChassisSpeeds speeds = drive.getChassisSpeeds();

Pose2d compensatedPose =
robotPose.exp(
new Twist2d(
speeds.vxMetersPerSecond * PHASE_DELAY_SECONDS,
speeds.vyMetersPerSecond * PHASE_DELAY_SECONDS,
speeds.omegaRadiansPerSecond * PHASE_DELAY_SECONDS));

Translation2d shooterPos = getShooterPosition(compensatedPose);
Translation2d fieldVelocity =
new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);

Translation2d aimPoint = targetPosition;
double timeOfFlight = 0.0;

for (int i = 0; i < CONVERGENCE_ITERATIONS; i++) {
double distance = aimPoint.minus(shooterPos).getNorm();
timeOfFlight = timeOfFlightMap.get(distance);
aimPoint = targetPosition.minus(fieldVelocity.times(timeOfFlight));
}

double finalDistance = aimPoint.minus(shooterPos).getNorm();

Logger.recordOutput("LeadComp/AimPoint", new Pose2d(aimPoint, Rotation2d.kZero));
Logger.recordOutput("LeadComp/Distance", finalDistance);
Logger.recordOutput("LeadComp/TOF", timeOfFlight);

return new LeadResult(
new Pose2d(aimPoint, aimPoint.minus(shooterPos).getAngle()), finalDistance, timeOfFlight);
}

public Pose2d getShootWhileDrivingResultPose() {
var shootWhileDrivingResult =
shooterLeadCompensator.shootWhileDriving(
AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
var result =
computeLeadCompensation(AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
return new Pose2d(
targetXFilter.calculate(shootWhileDrivingResult.target().getX()),
targetYFilter.calculate(shootWhileDrivingResult.target().getY()),
Rotation2d.fromDegrees(0));
targetXFilter.calculate(result.target().getX()),
targetYFilter.calculate(result.target().getY()),
Rotation2d.kZero);
}

public DoubleSupplier getShootWhileDrivingResultDistance() {
return () -> {
var shootWhileDrivingResult =
shooterLeadCompensator.shootWhileDriving(
AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
return shootWhileDrivingResult.distance();
};
return () ->
computeLeadCompensation(AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()))
.distance();
}

public DoubleSupplier getHubDistance() {
Expand All @@ -102,17 +138,13 @@ private Rotation2d filteredHubAngle(Rotation2d raw) {
}

public void orchestratorPeriodic() {
var shootWhileDrivingResult =
shooterLeadCompensator.shootWhileDriving(
AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
var result =
computeLeadCompensation(AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
Logger.recordOutput("Orchestrator/Target", result.target());
Logger.recordOutput("Orchestrator/DistanceToHub", result.distance());
Logger.recordOutput(
"Orchestrator/Target",
new Pose2d(
shootWhileDrivingResult.target().getX(),
shootWhileDrivingResult.target().getY(),
Rotation2d.fromDegrees(0)));
Logger.recordOutput("Orchestrator/DistanceToHub", shootWhileDrivingResult.distance());
Logger.recordOutput("Orchestrator/ShooterPosition", shooterLeadCompensator.shooterPose());
"Orchestrator/ShooterPosition",
new Pose2d(getShooterPosition(drive.getPose()), Rotation2d.kZero));
}

public Command driveToHub() {
Expand All @@ -123,11 +155,11 @@ public Command driveToHub() {
AllianceFlipUtil.apply(
Hub.nearFace.transformBy(
new Transform2d(FRONT_HUB_OFFSET, 0.0, Rotation2d.fromDegrees(0.0))));
Rotation2d sameAsAlignAndShootHeading =
Rotation2d heading =
AllianceFlipUtil.apply(Hub.blueCenter)
.minus(hubApproachPose.getTranslation())
.getAngle();
return new Pose2d(hubApproachPose.getTranslation(), sameAsAlignAndShootHeading);
return new Pose2d(hubApproachPose.getTranslation(), heading);
});
}

Expand All @@ -136,9 +168,10 @@ public Command alignToHub() {
drive,
() -> {
Pose2d hubApproachPose = AllianceFlipUtil.apply(Hub.nearFace);
Rotation2d angle =
hubApproachPose.getTranslation().minus(drive.getPose().getTranslation()).getAngle();
return angle;
return hubApproachPose
.getTranslation()
.minus(drive.getPose().getTranslation())
.getAngle();
});
}

Expand All @@ -165,7 +198,7 @@ public Command spinUpShooterDistance(DoubleSupplier targetDistance) {
return shooter.setTargetVelocityRadians(
() ->
Units.rotationsPerMinuteToRadiansPerSecond(
shooter.calculateSetpoint(targetDistance).getAsDouble()));
shooterRpmMap.get(targetDistance.getAsDouble())));
}

public Command spinUpShooterHub() {
Expand All @@ -188,24 +221,23 @@ public Command preloadBalls() {

public Command driveShootAtAngle() {
return Commands.parallel(
Commands.run(
() -> {
spinUpShooterDistance(this.getShootWhileDrivingResultDistance());
}),
Commands.run(() -> spinUpShooterDistance(this.getShootWhileDrivingResultDistance())),
DriveCommands.joystickDriveAtAngle(
drive,
driverController::getLeftX,
driverController::getLeftY,
() ->
(getShootWhileDrivingResultPose()
getShootWhileDrivingResultPose()
.getTranslation()
.minus(drive.getPose().getTranslation())
.getAngle())));
.getAngle()));
}

public Command spinUpShooterWhileDriving() {
return shooter.setTargetVelocityRadians(
() -> shooter.calculateSetpoint(this.getShootWhileDrivingResultDistance()).getAsDouble());
() ->
Units.rotationsPerMinuteToRadiansPerSecond(
shooterRpmMap.get(getShootWhileDrivingResultDistance().getAsDouble())));
}

public Command aimToHub() {
Expand Down
30 changes: 29 additions & 1 deletion src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
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.interpolation.InterpolatingDoubleTreeMap;

public class ShooterConstants {
public static final double ENCODER_POSITION_CONVERSION = 1;
Expand All @@ -22,8 +23,35 @@ public class ShooterConstants {

public static final double MAX_VOLTAGE = 12.0;

public static final double TOLERANCE = 50; // measured in radians
public static final double TOLERANCE = 50;

public static Transform2d kShooterOffsetFromRobotCenter =
new Transform2d(new Translation2d(-0.163, 0.0), new Rotation2d(0.0));

public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
public static final InterpolatingDoubleTreeMap shooterRpmMap = new InterpolatingDoubleTreeMap();

static {
timeOfFlightMap.put(1.50, 0.74);
timeOfFlightMap.put(1.90, 0.93);
timeOfFlightMap.put(2.23, 1.02);
timeOfFlightMap.put(2.39, 1.14);
timeOfFlightMap.put(2.55, 1.21);
timeOfFlightMap.put(2.70, 1.26);
timeOfFlightMap.put(2.82, 1.22);
timeOfFlightMap.put(3.17, 1.24);
timeOfFlightMap.put(3.36, 1.35);
timeOfFlightMap.put(4.02, 1.39);

shooterRpmMap.put(1.50, 1065.0);
shooterRpmMap.put(1.90, 1145.0);
shooterRpmMap.put(2.23, 1200.0);
shooterRpmMap.put(2.39, 1245.0);
shooterRpmMap.put(2.55, 1300.0);
shooterRpmMap.put(2.70, 1350.0);
shooterRpmMap.put(2.82, 1370.0);
shooterRpmMap.put(3.17, 1380.0);
shooterRpmMap.put(3.36, 1410.0);
shooterRpmMap.put(4.02, 1520.0);
}
}
67 changes: 0 additions & 67 deletions src/main/java/frc/robot/util/ShooterLeadCompensator.java
View file
Open in desktop

This file was deleted.

Loading