From 890a3de02a0a775ea8b3c4776651fe7a00a9928b Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 16 Feb 2024 21:49:50 -0700 Subject: [PATCH] Lots of bs (im geekin) --- src/main/java/frc4388/robot/Constants.java | 13 +- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 47 ++++- .../robot/commands/Autos/PlaybackChooser.java | 8 +- .../frc4388/robot/subsystems/Apriltags.java | 36 ---- .../java/frc4388/robot/subsystems/Intake.java | 43 +++-- .../java/frc4388/robot/subsystems/LED.java | 2 +- .../frc4388/robot/subsystems/Limelight.java | 169 +++--------------- .../frc4388/robot/subsystems/Shooter.java | 27 ++- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +- src/main/java/frc4388/utility/AprilTag.java | 13 -- 11 files changed, 135 insertions(+), 229 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java delete mode 100644 src/main/java/frc4388/utility/AprilTag.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7b5ba85..6b9c00d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -161,12 +161,13 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_MOTOR_ID = 18; //TODO: - public static final int PIVOT_MOTOR_ID = 17; //TODO: - public static final double INTAKE_SPEED = 0.75; //TODO: - public static final double INTAKE_OUT_SPEED = 0.1; - public static final double HANDOFF_SPEED = 0.4; //TODO: - public static final double PIVOT_SPEED = 0.2; //TODO: + public static final int INTAKE_MOTOR_ID = 18; + public static final int PIVOT_MOTOR_ID = 17; + public static final double INTAKE_SPEED = 0.75; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 0.7; + public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; + public static final double HANDOFF_SPEED = 0.4; + public static final double PIVOT_SPEED = 0.2; public static final class ArmPID { public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 44970b5..e5e850a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3fda500..4528086 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Intake; @@ -61,11 +62,9 @@ public class RobotContainer { private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private Limelight limelight = new Limelight(); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - - private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.pidIn()), new InstantCommand(() -> m_robotShooter.spin()) @@ -96,13 +95,34 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) ); - + /* Autos */ + private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private Command startLeftMoveRight = new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); + private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); - + private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( + ejectToShoot, + taxi + ); + private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( + startLeftMoveRight, + ejectToShoot, + taxi + ); + private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( + startRightMoveLeft, + ejectToShoot, + taxi + ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + .addOption("Taxi Auto", taxi) + .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) + .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) + .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) .buildDisplay(); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -122,7 +142,7 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); + // SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); // m_robotIntake.resetPostion(); } @@ -177,10 +197,19 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + // Override Intake Position encoder: out + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(-53), m_robotIntake)); + + // Override Intake Position encoder: out + new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(0), m_robotIntake)); + // //Pull arm in // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake)) @@ -202,7 +231,7 @@ public class RobotContainer { // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); //Spin Shooter Motors - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 2438a38..a270d7b 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -66,7 +66,13 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - for (String auto : m_dir.list()) { + String[] dirs = m_dir.list(); + + if(dirs == null){ // Fix funny error + return; + } + + for (String auto : dirs) { chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); } for (var cmd_name : m_commandPool.keySet()) { diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc4388.robot.subsystems; - -//import edu.wpi.first.apriltag.AprilTag; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.networktables.NetworkTable; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 4bf31f6..6cbb9e2 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -35,6 +35,8 @@ public class Intake extends SubsystemBase { private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; + + private double val; @@ -53,9 +55,6 @@ public class Intake extends SubsystemBase { reverseLimit.enableLimitSwitch(true); intakeMotor.restoreFactoryDefaults(); - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - - intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); @@ -76,7 +75,6 @@ public class Intake extends SubsystemBase { //Rotate robot in for handoff public void rotateArmIn() { - //pivot.set(IntakeConstants.PIVOT_SPEED); pivot.set(IntakeConstants.PIVOT_SPEED); } @@ -90,6 +88,10 @@ public class Intake extends SubsystemBase { m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); } + + public void pidOut() { + m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); + } public void limitNote() { if (intakeforwardLimit.isPressed()) { @@ -99,10 +101,6 @@ public class Intake extends SubsystemBase { } } - public void pidOut() { - m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); - } - public void rotateArmOut2() { if(reverseLimit.isPressed()){ stopArmMotor(); @@ -120,7 +118,15 @@ public class Intake extends SubsystemBase { } public void handoff() { - intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED); + intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } + + public void handoff2() { + if(intakeforwardLimit.isPressed()) { + intakeMotor.set(-val); + } else { + intakeMotor.set(-val); + } } public void stopIntakeMotors() { @@ -156,7 +162,11 @@ public class Intake extends SubsystemBase { } public void resetPostion() { - pivot.getEncoder().setPosition(0); + setPosition(0); + } + + public void setPosition(int val) { + pivot.getEncoder().setPosition(val); } public void resetPosition1() { @@ -169,6 +179,10 @@ public class Intake extends SubsystemBase { return pivot.getEncoder().getPosition(); } + public double getIntakeVelocity() { + return intakeMotor.getEncoder().getVelocity(); + } + public void rotateArm() { } @@ -181,11 +195,20 @@ public class Intake extends SubsystemBase { } } + public void changeIntakeNeutralState() { + if(forwardLimit.isPressed()) { + intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + } + } + @Override public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Vel Output", getVelocity()); SmartDashboard.putNumber("Position", getPos()); resetPosition1(); + changeIntakeNeutralState(); + + val = SmartDashboard.getNumber("Intake Speed", 0.5); } } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 85919b5..e9e070c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -33,7 +33,7 @@ public class LED extends SubsystemBase { if(m_self != null) return; m_led = new AddressableLED(9); - m_ledBuffer = new AddressableLEDBuffer(130); + m_ledBuffer = new AddressableLEDBuffer(10); m_led.setLength(m_ledBuffer.getLength()); m_led.setData(m_ledBuffer); m_led.start(); diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..989b55b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -3,163 +3,40 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; + +// Look at vvv for networktables stuff +// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data public class Limelight extends SubsystemBase { - - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; + private double[] cameraPose; + private Boolean isTag; - private boolean lightOn; - - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); + public boolean getIsTag() { + return isTag; } - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + public Pose2d getPose() { + //TODO - Get actual values! - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + double x = 0; + double y = 0; + double yaw = 0; - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } + Rotation2d rot = Rotation2d.fromDegrees(yaw); - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } - - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } - - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget(); - } - - private List getAprilCorners() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget().getDetectedCorners(); - } - - public double getAprilSkew() { - List corners = getAprilCorners(); - ArrayList bottomSide = getAprilBottomSide(corners); - - if (bottomSide == null) return 0; - - TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); - TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - - return bottomLeft.y - bottomRight.y; - } - - private ArrayList getAprilBottomSide(List box) { - if (box == null) return null; - - ArrayList bottomSide = new ArrayList<>(); - - TargetCorner l1 = new TargetCorner(-1, -1); - TargetCorner l2 = new TargetCorner(-1, -1); - - for (TargetCorner c : box) { - if (c.y > l1.y) l1 = c; - } - - for (TargetCorner c : box) { - if (c.y == l1.y) continue; - if (c.y > l2.y) l2 = c; - } - - bottomSide.add(l1); - bottomSide.add(l2); - - return bottomSide; - } - - public double getDistanceToApril() { - PhotonTrackedTarget aprilPoint = getAprilPoint(); - if (aprilPoint == null) return -1; - - double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + aprilPoint.getPitch(); - - double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); - return distanceToApril; - } - - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = (ArrayList) result.getTargets(); - - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } - - return lowest; - } - - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; - - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + tapePoint.getPitch(); - - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; + return new Pose2d(x, y, rot); } @Override - public void periodic() {} -} + public void periodic() { + // This method will be called once per scheduler run + + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); + cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8d340ed..6339fb2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -29,13 +30,31 @@ public class Shooter extends SubsystemBase { rightShooter.setNeutralMode(NeutralModeValue.Coast); } + public Shooter(TalonFX leftShooter) { + this.leftShooter = leftShooter; + leftShooter.setNeutralMode(NeutralModeValue.Coast); + } + + public void singleSpin() { + leftShooter.set(1.0); + } + + public void singleSpin(double speed) { + leftShooter.set(speed); + } + public void spin() { spin(ShooterConstants.SHOOTER_SPEED); } public void spin(double speed) { - leftShooter.set(-speed); - rightShooter.set(speed); + leftShooter.set(speed); + rightShooter.set(-speed); + } + + public void spin(double leftSpeed, double rightSpeed) { + leftShooter.set(leftSpeed); + rightShooter.set(-rightSpeed); } public void stop() { @@ -49,8 +68,8 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); - SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); + //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 86dd04b..8cb65d9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -58,7 +58,7 @@ public class SwerveDrive extends SubsystemBase { if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - SmartDashboard.putBoolean("drift correction", false); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -66,7 +66,7 @@ public class SwerveDrive extends SubsystemBase { stopped = true; } - SmartDashboard.putBoolean("drift correction", true); + // SmartDashboard.putBoolean("drift correction", true); rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -}