diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 54f68d3..ac78650 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -22,7 +22,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; import frc4388.robot.subsystems.Location; - +import frc4388.robot.subsystems.Apriltags.Tag; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -71,12 +71,9 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // Call position and rotation reorder - location.reoderPrio(); - - final Object[] pos = location.getPosition(); + final Tag pos = location.getPosRot(); if (pos != null) { - SmartDashboard.putNumber("name", (Double) pos[0]); + SmartDashboard.putNumber("x position", pos.x); } //ystem.out.print(apriltagPos[0]); diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java index bc24769..c6062e8 100644 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -9,41 +9,28 @@ import edu.wpi.first.networktables.NetworkTableInstance; public class Apriltags { public static class Tag { - boolean visible = true; - double x, y, z = 0; - } - - public Object[] getApriltagPosition() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - final var tag = new Tag(); - // integrate the tag system - - // return new Object[] {true, - // tagTable.getEntry("TagPosX"), - // tagTable.getEntry("TagPosY"), - // tagTable.getEntry("TagPosZ") - // }; - - return new Object[] {true, - 1, - 2, - 3 - }; + public boolean visible = true; + public double x, y, z = 0; + public double ry, rp, rr = 0; } - public Object[] getApriltagRotation() { + public Tag getTagPosRot() { final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return new Object[] {true, - tagTable.getEntry("TagRotY"), - tagTable.getEntry("TagRotP"), - tagTable.getEntry("TagRotR") - }; + 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); - return true; + return tagTable.getEntry("IsTag").getBoolean(false); } } diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java index e5d6959..0bf5322 100644 --- a/src/main/java/frc4388/robot/subsystems/Location.java +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -1,40 +1,27 @@ package frc4388.robot.subsystems; -import frc4388.robot.subsystems.Apriltags; +import frc4388.robot.subsystems.Apriltags.Tag; public class Location { - final Apriltags Apriltag = new Apriltags(); + final Apriltags apriltag = new Apriltags(); private boolean isLimelight = false; private boolean isApriltag = false; //Determines which source to get pos and rot from and also resets - public void reoderPrio(){ + private void reoderPrio(){ isLimelight = false; //If limelight gets position and if within a certain range of poles - isApriltag = Apriltag.isAprilTag(); + isApriltag = apriltag.isAprilTag(); } - public Object[] getPosition() { - if(isLimelight){ - //Return Limelight Position - }else if(isApriltag){ - return Apriltag.getApriltagPosition(); + public Tag getPosRot() { + reoderPrio(); + if(isApriltag){ + return apriltag.getTagPosRot(); + } else if (isLimelight) { + return null; } return null; } - - public Object[] getRotation() { - Object[] Rotation = {}; - - if(isLimelight){ - //Return Limelight Rotation - }else if(isApriltag){ - return Apriltag.getApriltagRotation(); - }else{ - //Return odometry Rotation, last resort - } - - return Rotation; - } }