Add support for orin on reva?!?!?!?

Team766 · Oct 24, 2024 · 4899d3c · 4899d3c
1 parent 2eefeaa
commit 4899d3c
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 3 deletions.
diff --git a/src/main/java/com/team766/orin/NoTagFoundError.java b/src/main/java/com/team766/orin/NoTagFoundError.java
@@ -0,0 +1,8 @@
+package com.team766.orin;
+
+public class NoTagFoundError extends Exception{
+    public NoTagFoundError(int id){
+        super("No tag found on table with given id of: " + id);
+    }
+
+}
diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Orin.java b/src/main/java/com/team766/robot/reva/mechanisms/Orin.java
@@ -3,6 +3,7 @@
 import java.util.ArrayList;
 import com.team766.framework.Mechanism;
 import com.team766.orin.GetApriltagPoseData;
+import com.team766.orin.NoTagFoundError;
 import com.team766.robot.reva.Robot;
 import edu.wpi.first.apriltag.AprilTag;
 
@@ -11,6 +12,16 @@ public Orin(){
 
     }
 
+    public AprilTag getTagById (int id) throws NoTagFoundError{
+        ArrayList<AprilTag> tags = GetApriltagPoseData.getAllTags();
+
+        for (AprilTag tag : tags){
+            if (tag.ID == id) return tag;
+        }
+
+        throw new NoTagFoundError(id);
+    }
+
     public void run(){
         ArrayList<AprilTag> tags = GetApriltagPoseData.getAllTags();
 

diff --git a/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java b/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java
@@ -4,9 +4,13 @@
 import com.team766.ViSIONbase.GrayScaleCamera;
 import com.team766.framework.Context;
 import com.team766.logging.LoggerExceptionUtils;
+import com.team766.orin.NoTagFoundError;
 import com.team766.robot.reva.Robot;
 import com.team766.robot.reva.VisionUtil.VisionPIDProcedure;
 import com.team766.robot.reva.constants.VisionConstants;
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -40,9 +44,10 @@ public void run(Context context) {
 
         Transform3d toUse;
         try {
-            toUse = getTransform3dOfRobotToTag();
-
-        } catch (AprilTagGeneralCheckedException e) {
+            /* Interchange the following two lines for Orin vs. Orange Pi! */
+            //toUse = getTransform3dOfRobotToTag();
+            toUse = getTransform3dOfRobotToTagOrin();
+        } catch (NoTagFoundError e) {
             LoggerExceptionUtils.logException(e);
             return;
         }
@@ -117,4 +122,13 @@ private Transform3d getTransform3dOfRobotToTag() throws AprilTagGeneralCheckedEx
 
         return GrayScaleCamera.getBestTargetTransform3d(toUse.getTrackedTargetWithID(tagId));
     }
+
+    private Transform3d getTransform3dOfRobotToTagOrin() throws NoTagFoundError {
+        AprilTag tag = Robot.orin.getTagById(tagId);
+
+        Pose3d pose = tag.pose;
+
+        Transform3d poseNew = new Transform3d(pose.getX(),pose.getY(), pose.getZ(), new Rotation3d());
+        return poseNew;
+    }
 }