From 1c64d7344b83237c32af41a1d632e2692239aeab Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Fri, 21 Feb 2025 04:22:22 -0500 Subject: [PATCH] vision stuff --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/subsystems/Vision.java | 18 ++++++++---------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bebe34e..4ec5db5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,9 +52,10 @@ public class RobotContainer { climberRollers = new ClimberRollers(); - //vision = new Vision(drivetrain::getGyroValue); drivetrain = new Drivetrain(); + vision = new Vision(drivetrain::getGyroValue); + elevator = new Elevator(); //elevator = new ElevatorSysID(); diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 7398bf5..1823137 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -40,9 +40,9 @@ public class Vision{ NetworkTable blackVisionTable = inst.getTable("black_Fiducial"); NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial"); - blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0); - blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0); - blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0); + blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0); + blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0); + blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0); blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0); blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false); @@ -59,24 +59,23 @@ public class Vision{ orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0); } - public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){ + public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){ Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0], VisionConstants.globalTagCoords[tagID][1], new Rotation2d()); - Pose2d relative = new Pose2d(relativeCoords, gyroAngle); + Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble())); Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation()); Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse()); - return new Pose2d(globalPose.getTranslation(), gyroAngle); + return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble())); } public Pose2d getBlackGlobalPose(){ return relativeToGlobalPose2d(getBlackClosestTag(), - new Translation2d(getBlackRelativeX(), getBlackRelativeY()), - new Rotation2d(gyroAngle.getAsDouble())); + new Translation2d(getBlackRelativeX(), getBlackRelativeY())); } public double getBlackRelativeX(){ @@ -109,8 +108,7 @@ public class Vision{ public Pose2d getOrangeGlobalPose(){ return relativeToGlobalPose2d(getOrangeClosestTag(), - new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), - new Rotation2d(gyroAngle.getAsDouble())); + new Translation2d(getOrangeRelativeX(), getOrangeRelativeY())); } public double getOrangeRelativeX(){