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. *