From ee113239d12b3cd8a1ae9dd7af9d74a4167444d6 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Sat, 4 Feb 2023 13:48:05 -0700 Subject: [PATCH 1/3] added april tags --- simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 55 +++++++++++ .../java/frc4388/robot/subsystems/Vision.java | 39 ++++++++ 3 files changed, 186 insertions(+) create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..c0596a7 --- /dev/null +++ b/simgui.json @@ -0,0 +1,55 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables": { + "transitory": { + "LiveWindow": { + ".status": { + "open": true + } + }, + "Shuffleboard": { + ".recording": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "NT3@192.168.1.132:40708": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "NT3@192.168.1.132:49546": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "NT3@192.168.1.132:52724": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "visible": true + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..b5fe03d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,39 @@ +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.NetworkTable; +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; + } +} From c536ad69bfc3c59a8a26136b1117d5608d3e6e8a Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Sat, 4 Feb 2023 15:38:37 -0700 Subject: [PATCH 2/3] Get NetworkTables Working --- src/main/java/frc4388/robot/Robot.java | 9 ++++++++- src/main/java/frc4388/robot/subsystems/Vision.java | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 8fc1ca3..16c67de 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,11 @@ package frc4388.robot; + +import frc4388.robot.subsystems.Vision; + +import java.util.Arrays; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -24,7 +29,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - + private Vision Vision = new Vision(); /** * This function is run when the robot is first started up and should be @@ -35,6 +40,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + } /** @@ -53,6 +59,7 @@ public class Robot extends TimedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + System.out.println(Arrays.toString(Vision.getAprilTags())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b5fe03d..3731849 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -24,7 +24,7 @@ public class Vision { 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[] {}); From 3b1b6e7378e8af43cb83f4ecb2790e545ca8bca2 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 6 Feb 2023 17:22:37 -0700 Subject: [PATCH 3/3] fefef --- simgui.json | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/simgui.json b/simgui.json index c0596a7..64da7ff 100644 --- a/simgui.json +++ b/simgui.json @@ -5,6 +5,11 @@ } }, "NetworkTables": { + "retained": { + "apriltag": { + "open": true + } + }, "transitory": { "LiveWindow": { ".status": {