From 3f26c2eaf2658cffb8f95cfa7c2eadc7ecb75a0d Mon Sep 17 00:00:00 2001 From: mimigamin Date: Wed, 11 Mar 2026 22:00:57 -0600 Subject: [PATCH] Recommit --- .../java/frc4388/robot/subsystems/swerve/SwerveDrive.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index fa4a750..54e77cd 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -391,8 +391,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Aim Offset", aimOffset); + + // double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond))*Math.sqrt(1+Math.pow(7.161/distanceToHub, 2)); + // fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); + // Logger.recordOutput("Aim Offset", aimOffset); } + + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();