From 2692f8667679ce47cbf539ffdd67d670c29be370 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 6 Feb 2024 16:00:16 -0600 Subject: [PATCH] Added photonvision example function. Signed-off-by: thenetworkgrinch --- .../swervedrive/SwerveSubsystem.java | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 7fd5106d..b0a65f47 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -24,6 +24,8 @@ import frc.robot.Constants.AutonConstants; import java.io.File; import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -127,6 +129,26 @@ public void setupPathPlanner() ); } + /** + * Aim the robot at the target returned by PhotonVision. + * + * @param camera {@link PhotonCamera} to communicate with. + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(PhotonCamera camera) + { + return run(() -> { + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets()) + { + drive(getTargetSpeeds(0, + 0, + Rotation2d.fromDegrees(result.getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } + }); + } + /** * Get the path follower with events. *