Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

global apriltag coordinates

Browse Source
This commit is contained in:
Tylr-J42 2025-02-15 01:48:58 -05:00
parent 2275248f70
commit 38dad2861d
2 changed files with 42 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/main/java/frc/robot/constants/VisionConstants.java Normal file
View File

@ -0,0 +1,31 @@
package frc.robot.constants;
public class VisionConstants {
// global coordinate map of all tags. index is the tag id.
// Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
// This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
public static final double[][] globalTagCoords = {{},
{656.98, 24.73, 58.50, 126.0, 0},
{656.98, 291.90, 58.50, 234.0, 0},
{452.4, 316.21, 51.25, 270, 0},
{365.2, 241.44, 73.54, 0, 30},
{365.2, 75.19, 73.54, 0, 30},
{530.49, 129.97, 12.13, 300, 0},
{546.87, 158.3, 12.13, 0, 0},
{530.49, 186.63, 12.13, 60, 0},
{497.77, 186.63, 12.13, 120, 0},
{481.39, 158.3, 12.13, 180, 0},
{497.77, 129.97, 12.13, 240, 0},
{33.9, 24.73, 58.5, 54, 0},
{33.9, 291.9, 58.5, 306, 0},
{325.68, 241.44, 73.54, 180, 30},
{325.68, 75.19, 73.54, 180, 30},
{238.49, 0.42, 51.25, 90, 0},
{160.39, 129.97, 12.13, 240, 0},
{144.00, 158.3, 12.13, 180, 0},
{160.39, 186.63, 12.13, 120, 0},
{193.1, 186.63, 12.13, 60, 0},
{209.49, 158.3, 12.13, 0, 0},
{193.1, 129.97, 12.13, 300, 0},
};
}

16
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -13,7 +13,10 @@ public class Vision{
private DoubleSubscriber blackFramerate;
private DoubleSubscriber orangeRobotRelativePose;
private DoubleSubscriber orangeRobotRelativeX;
private DoubleSubscriber orangeRobotRelativeY;
private DoubleSubscriber orangeRobotRelativeZ;
private DoubleSubscriber orangeClosestTag;
private BooleanSubscriber orangeTagDetected;
@ -31,7 +34,10 @@ public class Vision{
blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
orangeRobotRelativePose = orangeVisionTable.getDoubleTopic("orangeRelativePose").subscribe(0.0);
orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
@ -58,8 +64,8 @@ public class Vision{
return blackFramerate.get();
}
public double getOrangeGlobalPose(){
return orangeRobotRelativePose.get();
public double getOrangeGlobalX(){
return orangeRobotRelativeX.get();
}
public double getOrangeClosestTag(){
@ -67,7 +73,7 @@ public class Vision{
}
public double getOrangeTimeStamp(){
return orangeRobotRelativePose.getLastChange();
return orangeRobotRelativeX.getLastChange();
}
public boolean getOrangeTagDetected(){