Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Make boiler plate code - Needs to be programmed with robot

Browse Source
This commit is contained in:
Astatin3
2024-02-15 10:27:44 -07:00
parent 19fb13e5d9
commit 1d04fc34de
11 changed files with 125 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Vision.java
-38
View File
@@ -1,38 +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.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Vision {
private final NetworkTableEntry m_isTags;
private final NetworkTableEntry m_xPoses;
private final NetworkTableEntry m_yPoses;
private final NetworkTableEntry m_zPoses;
public Vision() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
m_isTags = tagTable.getEntry("IsTag");
m_xPoses = tagTable.getEntry("TagPosX");
m_yPoses = tagTable.getEntry("TagPosY");
m_zPoses = tagTable.getEntry("TagPosZ");
}
public AprilTag[] getAprilTags() {
if (!m_isTags.getBoolean(false)) return new AprilTag[0];
double xarr[] = m_xPoses.getDoubleArray(new double[] {});
double yarr[] = m_yPoses.getDoubleArray(new double[] {});
double zarr[] = m_zPoses.getDoubleArray(new double[] {});
AprilTag tags[] = new AprilTag[xarr.length];
for (int i = 0; i < tags.length; i++) {
tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
}
return tags;
}
}