diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17af840..3e3f6c2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -321,17 +321,34 @@ public class RobotContainer { //sensorTab.add("odometry", drivetrain::getPose); - apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag()); - apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()); - apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()); - apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()); - apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX()); - apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY()); - apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag()); - apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()); + apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag()) + .withSize(1,1).withPosition(1,1); + apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()) + .withSize(2,1).withPosition(2,1); + apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()) + .withSize(3,1).withPosition(3,1); + apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()) + .withSize(4,1).withPosition(4,1); + apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()) + .withSize(6,1).withPosition(5,1); + apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected()) + .withSize(7,1).withPosition(6,1); -// sensorTab.addDouble(" ID", vision::getOrangeClosestTag); + apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag()) + .withSize(1,1).withPosition(1,2); + apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX()) + .withSize(1,1).withPosition(2,2); + apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY()) + .withSize(1,1).withPosition(3,2); + apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist()) + .withSize(1,1).withPosition(4,2); + apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS()) + .withSize(1,1).withPosition(5,2); + apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected()) + .withSize(1,1).withPosition(6,2); + apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag()) + .withSize(1,1).withPosition(4,4); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a58b6f7..e22cfdc 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -210,8 +210,6 @@ public class Drivetrain extends SubsystemBase { } } Logger.recordOutput("black pose", new Pose3d(blackPose2d)); - - Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition())); @@ -259,11 +257,12 @@ public class Drivetrain extends SubsystemBase { public void resetOdometry(Pose2d pose) { m_estimator.resetPosition( Rotation2d.fromDegrees(getGyroValue()), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition()}, + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }, pose ); }