diff --git a/yagsl/java/swervelib/SwerveInputStream.java b/yagsl/java/swervelib/SwerveInputStream.java index e895911..9bcfca6 100644 --- a/yagsl/java/swervelib/SwerveInputStream.java +++ b/yagsl/java/swervelib/SwerveInputStream.java @@ -96,7 +96,7 @@ public class SwerveInputStream implements Supplier /** * Target to aim at. */ - private Optional aimTarget = Optional.empty(); + private Optional> aimTarget = Optional.empty(); /** * Target {@link Supplier} to drive towards when driveToPose is enabled. */ @@ -570,7 +570,19 @@ public SwerveInputStream headingWhile(boolean headingState) */ public SwerveInputStream aim(Pose2d aimTarget) { - this.aimTarget = aimTarget.equals(Pose2d.kZero) ? Optional.empty() : Optional.of(aimTarget); + aim(() -> aimTarget); + return this; + } + + /** + * Aim the {@link SwerveDrive} at this pose while driving. + * + * @param aimTarget {@link Supplier} to point at. + * @return this + */ + public SwerveInputStream aim(Supplier aimTarget) + { + this.aimTarget = aimTarget.get().equals(Pose2d.kZero) ? Optional.empty() : Optional.of(aimTarget); return this; } @@ -1120,7 +1132,7 @@ public ChassisSpeeds get() } case AIM -> { - var targetVector = getTargetVector(aimTarget.orElseThrow()); + var targetVector = getTargetVector(aimTarget.orElseThrow().get()); var targetDistance = targetVector.getNorm(); // TODO: Shoot on the move, using // targetVector = targetVector.div(targetDistance).times(sotmDistanceToRPSMap.get(targetDistance)*flyWheelCircumference)