From 514aaa7a266c8d63335a6a5ec95c652b07892e03 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Fri, 27 Mar 2026 22:53:58 -0400 Subject: [PATCH] new lead logic --- src/main/java/frc/robot/Orchestrator.java | 125 +++++++++++------- .../subsystems/shooter/ShooterConstants.java | 30 ++++- .../robot/util/ShooterLeadCompensator.java | 67 ---------- 3 files changed, 107 insertions(+), 115 deletions(-) delete mode 100644 src/main/java/frc/robot/util/ShooterLeadCompensator.java diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 18e23b23c..d5b00a1b3 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -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; @@ -21,30 +24,25 @@ import frc.robot.subsystems.intake.Intake; 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 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, @@ -58,27 +56,64 @@ public Orchestrator( this.shooter = shooter; this.indexer = indexer; this.intake = intake; - 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() { @@ -98,17 +133,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() { @@ -119,11 +150,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); }); } @@ -132,9 +163,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(); }); } @@ -162,7 +194,7 @@ public Command spinUpShooterDistance(DoubleSupplier targetDistance) { return shooter.setTargetVelocityRadians( () -> Units.rotationsPerMinuteToRadiansPerSecond( - shooter.calculateSetpoint(targetDistance).getAsDouble())); + shooterRpmMap.get(targetDistance.getAsDouble()))); } public Command spinUpShooterHub() { @@ -185,24 +217,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() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index b5d07b404..dc6fedfc6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -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; @@ -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); + } } diff --git a/src/main/java/frc/robot/util/ShooterLeadCompensator.java b/src/main/java/frc/robot/util/ShooterLeadCompensator.java deleted file mode 100644 index 2405d0979..000000000 --- a/src/main/java/frc/robot/util/ShooterLeadCompensator.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.robot.util; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; -import java.util.function.Supplier; - -public class ShooterLeadCompensator { - - private final Supplier shooterPosition; - private final Supplier robotVelocity; - private final Shooter shooter; - - public ShooterLeadCompensator(Drive drive, Shooter shooter) { - this.shooterPosition = - () -> { - Pose2d robotPose = drive.getPose(); - Translation2d rotatedOffset = - ShooterConstants.kShooterOffsetFromRobotCenter - .getTranslation() - .rotateBy(robotPose.getRotation()); // robot-relative → field-relative - return robotPose.getTranslation().plus(rotatedOffset); // add in field coords - }; - - this.robotVelocity = - () -> - new Translation2d( - drive.getChassisSpeeds().vxMetersPerSecond, - drive.getChassisSpeeds().vyMetersPerSecond); - - this.shooter = shooter; - } - - public ShootWhileDrivingResult shootWhileDriving(Translation2d targetPosition) { - Translation2d shooterPos = shooterPosition.get(); - Translation2d v = robotVelocity.get(); - - Translation2d aimPoint = targetPosition; - - /** - * Our function for defining our target pose is dependant on the distance it takes for the ball - * to reach the final pose. This circular reasoning is a problem. To fix it we assume that - * distance is equal to our current "target" and adjust our target pose by the distance to the - * current "target." After around 10 iterations we will have a very good approximation for the - * optimal target. - */ - for (int i = 0; i < 10; i++) { - double distance = aimPoint.minus(shooterPos).getNorm(); - double t = shooter.getAirTimeSeconds(() -> distance); - aimPoint = targetPosition.plus(v.times(t)); - } - - double finalDistance = aimPoint.minus(shooterPos).getNorm(); - Pose2d aimPose = new Pose2d(aimPoint, aimPoint.getAngle()); - - return new ShootWhileDrivingResult(aimPose, finalDistance); - } - - public Pose2d shooterPose() { - return new Pose2d(shooterPosition.get().getX(), shooterPosition.get().getY(), Rotation2d.kZero); - } - - public record ShootWhileDrivingResult(Pose2d target, double distance) {} -}