diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java index c348bc2..8e1b842 100644 --- a/src/main/java/frc/robot/commands/AimAtTag.java +++ b/src/main/java/frc/robot/commands/AimAtTag.java @@ -5,9 +5,9 @@ import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.Vision; -import org.photonvision.targeting.PhotonTrackedTarget; - import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -18,17 +18,19 @@ public class AimAtTag extends Command { private final ChassisDriveInputs chassisDriveInputs; private final Vision vision; - private final Integer tagID; // Integer as opposed to int so it can be null for best tag + private final Integer tagID; // Integer as opposed to int so it can be null for best tag private final PIDController rotatePID; /** - * Create a new AimAtTag command. Tries to constants aim at a tag while still allowing driver to control robot. + * Create a new AimAtTag command. Tries to constants aim at a tag while still + * allowing driver to control robot. * * @param drivetrain the drivetrain of the robot * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to turn to, null for best tag - * @param chassisDriveControl collection of inputs for driving + * @param tagID the numerical ID of the the tag to turn to, null + * for best tag + * @param chassisDriveControl collection of inputs for driving */ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, ChassisDriveInputs chassisDriveInputs) { this.drivetrain = drivetrain; @@ -37,12 +39,12 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass this.tagID = tagID; this.chassisDriveInputs = chassisDriveInputs; - rotatePID = new PIDController( - 2, - 0, - 0); - rotatePID.enableContinuousInput(-1, 1); - rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS)); + rotatePID = new PIDController( + 2, + 0, + 0); + rotatePID.enableContinuousInput(-1, 1); + rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS)); rotatePID.setSetpoint(0); addRequirements(drivetrain); @@ -51,9 +53,10 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass /** * Create a new AimAtTag command. Tries to aim at a tag. * - * @param drivetrain the drivetrain of the robot - * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to turn to, null for best tag + * @param drivetrain the drivetrain of the robot + * @param vision the vision subsystem of the robot + * @param tagID the numerical ID of the the tag to turn to, null for best + * tag */ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) { this(drivetrain, vision, tagID, null); @@ -66,11 +69,12 @@ public void initialize() { @Override public void execute() { - PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID); + Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID); double tagYawRadians = 0; - if (tag != null) { - tagYawRadians = Units.degreesToRadians(tag.getYaw()); + if (distToTag != null) { + Rotation2d angleToTag = new Rotation2d(distToTag.getX(), distToTag.getY()); + tagYawRadians = angleToTag.getRadians(); } double xSpeed = 0; @@ -79,8 +83,8 @@ public void execute() { xSpeed = chassisDriveInputs.getX(); ySpeed = chassisDriveInputs.getY(); } - double rotateSpeed = rotatePID.calculate(tagYawRadians); - + double rotateSpeed = rotatePID.calculate(tagYawRadians); + ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed); drivetrain.setDesiredState(desiredSpeeds, false, true); @@ -97,4 +101,4 @@ public boolean isFinished() { public void end(boolean interrupted) { drivetrain.stop(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java index 9c7d2c1..80e4ef5 100644 --- a/src/main/java/frc/robot/commands/FollowTag.java +++ b/src/main/java/frc/robot/commands/FollowTag.java @@ -3,8 +3,6 @@ import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.Vision; -import org.photonvision.targeting.PhotonTrackedTarget; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; @@ -17,10 +15,10 @@ public class FollowTag extends Command { private final SwerveDrivetrain drivetrain; private final Vision vision; - private final Integer tagID; // Integer as opposed to int so it can be null for best tag + private final Integer tagID; // Integer as opposed to int so it can be null for best tag private final Transform2d targetDistance; - private final Double loseTagAfterSeconds; // Double as opposed to double so it can be null for never lose mode. + private final Double loseTagAfterSeconds; // Double as opposed to double so it can be null for never lose mode. private double secondsSinceTagLastSeen; /** @@ -29,9 +27,11 @@ public class FollowTag extends Command { * * @param drivetrain the drivetrain of the robot * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to follow, null for best tag + * @param tagID the numerical ID of the the tag to follow, null + * for best tag * @param targetDistanceToTag the target distance away from the tag to be - * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish + * @param loseTagAfterSeconds how long to wait before giving up on rediscover + * tag, set to null to never finish */ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) { @@ -52,7 +52,8 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetD * * @param drivetrain the drivetrain of the robot * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to follow, null for whatever best is + * @param tagID the numerical ID of the the tag to follow, null + * for whatever best is * @param targetDistanceToTag the target distance away from the tag to be * @param loseTagAfterSeconds how long to wait before giving up on rediscover * tag, set to null to never finish @@ -70,23 +71,22 @@ public void initialize() { @Override public void execute() { - - final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID); - if (tag == null) { + final Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID); + + if (distToTag == null) { secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod; drivetrain.stop(); } else { - final Transform3d tagPosition3d = vision.getDistanceToTarget(tag); secondsSinceTagLastSeen = 0; // https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ() final Transform2d tagPosition = new Transform2d( - tagPosition3d.getZ(), - tagPosition3d.getX(), - Rotation2d.fromRadians(tag.getYaw())); + distToTag.getZ(), + distToTag.getX(), + Rotation2d.fromRadians(distToTag.getRotation().getZ())); final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse()); @@ -106,4 +106,4 @@ public void end(boolean interrupted) { drivetrain.clearDesiredPosition(); drivetrain.stop(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index c685379..58ffec8 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -18,52 +18,51 @@ public class Vision extends SubsystemBase { private final static boolean DEBUG_INFO = false; final PhotonCamera camera; - final Transform3d cameraToFrontCenter; + final Transform3d robotToCamera; + final Transform3d cameraToRobot; /** * Create new PhotonCamera subsystem * - * @param cameraName name of PhotonCamera - * @param cameraToFrontCenter distance from the camera to the front center point - * of the robot + * @param cameraName name of PhotonCamera + * @param robotToCamera distance from the camera to the center of the robot */ - public Vision(String cameraName, Transform3d cameraToFrontCenter) { + public Vision(String cameraName, Transform3d robotToCamera) { camera = new PhotonCamera(cameraName); - this.cameraToFrontCenter = cameraToFrontCenter; + this.robotToCamera = robotToCamera; + cameraToRobot = robotToCamera.inverse(); } /** - * Get best april tag target + * Get distance to the distance to the best tag found by the camera + * + * @return The position of the tag (translation and rotation) based on the + * center of the robot. Returns null if no tag found. * - * @return Object of best target */ - public PhotonTrackedTarget getTag() { - return camera - .getLatestResult() - .getBestTarget(); + public Transform3d getDistToTag() { + PhotonTrackedTarget target = camera.getLatestResult().getBestTarget(); + if (target == null) { + return null; + } + return target.getBestCameraToTarget().plus(cameraToRobot); } /** - * Gives Transform3d from robot center to the desired target + * Get distance to the desired tag * - * @param tagID The fiducial ID of the desired April Tag - * @return returns first tag with matching ID, null if None are found + * @param tagID the fiducial ID of the desired tag + * @return the position of the tag (translation and rotation) based on the + * center of the robot. Returns null if no tag found */ - public PhotonTrackedTarget getTag(int tagID) { + public Transform3d getDistToTag(int tagID) { for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { if (target.getFiducialId() == tagID) - return target; + return target.getBestCameraToTarget().plus(cameraToRobot); } return null; } - public Transform3d getDistanceToTarget(PhotonTrackedTarget tag) { - return tag - .getBestCameraToTarget() - .plus(cameraToFrontCenter); - } - - @Override public void periodic() { if (!DEBUG_INFO) @@ -96,4 +95,4 @@ public void periodic() { SmartDashboard.putNumber("Tag Pose Pitch", tagPoseRotation.getY()); SmartDashboard.putNumber("Tag Pose Roll", tagPoseRotation.getX()); } -} +} \ No newline at end of file