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