make test mechanism to test code

src/main/java/com/team766/robot/reva/Robot.java

@@ -12,6 +12,7 @@
 import com.team766.robot.reva.mechanisms.Intake;
 import com.team766.robot.reva.mechanisms.Lights;
 import com.team766.robot.reva.mechanisms.NoteCamera;
+import com.team766.robot.reva.mechanisms.Orin;
 import com.team766.robot.reva.mechanisms.Shooter;
 import com.team766.robot.reva.mechanisms.Shoulder;
 
@@ -26,6 +27,7 @@ public class Robot implements RobotConfigurator {
     public static ForwardApriltagCamera forwardApriltagCamera;
     public static NoteCamera noteCamera;
     public static Lights lights;
+    public static Orin orin;
 
     @Override
     public void initializeMechanisms() {
@@ -42,6 +44,7 @@ public void initializeMechanisms() {
         } catch (AprilTagGeneralCheckedException e) {
             LoggerExceptionUtils.logException(e);
         }
+        orin = new Orin();
     }
 
     @Override

src/main/java/com/team766/robot/reva/mechanisms/Lights.java

@@ -86,6 +86,11 @@ public boolean signalStartingShootingProcedure() {
         return handleErrorCode(e);
     }
 
+    public boolean signalHasTag(){
+        ErrorCode e = m_candle.setLEDs(255, 215, 0);
+        return handleErrorCode(e);
+    }
+
     private boolean handleErrorCode(ErrorCode e) {
         if (e.equals(ErrorCode.OK)) {
             return true;

src/main/java/com/team766/robot/reva/mechanisms/Orin.java

@@ -0,0 +1,22 @@
+package com.team766.robot.reva.mechanisms;
+
+import java.util.ArrayList;
+import com.team766.framework.Mechanism;
+import com.team766.orin.GetApriltagPoseData;
+import com.team766.robot.reva.Robot;
+import edu.wpi.first.apriltag.AprilTag;
+
+public class Orin extends Mechanism {
+    public Orin(){
+
+    }
+
+    public void run(){
+        ArrayList<AprilTag> tags = GetApriltagPoseData.getAllTags();
+
+        if(tags.size() > 0){
+            Robot.lights.signalHasTag();
+        }
+    }
+
+}