add trap code

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

@@ -0,0 +1,85 @@
+package com.team766.robot.reva.mechanisms;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+import com.team766.ViSIONbase.AprilTagGeneralCheckedException;
+import com.team766.ViSIONbase.GrayScaleCamera;
+import com.team766.controllers.PIDController;
+import com.team766.framework.Mechanism;
+import com.team766.robot.reva.Robot;
+import edu.wpi.first.math.geometry.Transform3d;
+
+
+public class ArmCamera extends Mechanism {
+        private GrayScaleCamera camera;
+        private int[] tagIds = {11,12,13,14,15,16};
+        private double targetX = 0;
+        private double targetZ = 0;
+        private PIDController xPID = new PIDController(0,0,0,0,0,0);
+        private PIDController zPID = new PIDController(0,0,0,0,0,0);
+
+        public ArmCamera() throws AprilTagGeneralCheckedException {
+            try{
+                camera = new GrayScaleCamera("Arm_Camera_2024");
+            } catch (Exception e) {
+                log("Unable to create GrayScaleCamera for Arm Camera");
+            }
+            xPID.setSetpoint(targetX);
+            zPID.setSetpoint(targetZ);
+        }
+
+        public GrayScaleCamera getCamera() {
+            return camera;
+        }
+
+        public void shootTrap(){
+            PhotonTrackedTarget target = null;
+            int tagIdUsing = 0;
+            for (int tagId: tagIds){
+                try{
+                    target = camera.getTrackedTargetWithID(tagId);
+                    tagIdUsing = tagId;
+                    break;
+                } catch (Exception e){
+                    log("Unable to get target with ID: " + tagId);
+                    if (tagId == tagIds[tagIds.length - 1]){
+                        Robot.lights.signalTrapNotWorking();
+                        return;
+                    }
+                }
+            }
+
+            Transform3d toUse = GrayScaleCamera.getBestTargetTransform3d(target);
+
+            double currentX = toUse.getTranslation().getX();
+            double currentZ = toUse.getTranslation().getZ();
+
+
+            xPID.calculate(currentX);
+            zPID.calculate(currentZ);
+
+            while (Math.abs(xPID.getOutput()) + Math.abs(zPID.getOutput()) > 0){
+                try{
+                    toUse = GrayScaleCamera.getBestTargetTransform3d(camera.getTrackedTargetWithID(tagIdUsing));
+                } catch (Exception e){
+                    log("Lost tag!");
+                    Robot.lights.signalTrapNotWorking();
+                    return;
+                }
+                currentX = toUse.getTranslation().getX();
+                currentZ = toUse.getTranslation().getZ();
+
+                xPID.calculate(currentX);
+                zPID.calculate(currentZ);
+
+                Robot.drive.controlRobotOriented(currentX, currentZ, 0);
+                Robot.lights.signalTrapRunning();
+            }
+            Robot.lights.signalTrapFinished();
+
+        }
+
+
+
+
+
+}

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

@@ -27,7 +27,7 @@ public ForwardApriltagCamera() throws AprilTagGeneralCheckedException {
                 Robot.lights.signalCameraNotConnected();
             }
         } catch (Exception e) {
-            log("Unable to create GrayScaleCamera");
+            log("Unable to create GrayScaleCamera for Apriltag Camera");
             LoggerExceptionUtils.logException(e);
             Robot.lights.signalCameraNotConnected();
         }

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

@@ -85,6 +85,21 @@ public boolean signalStartingShootingProcedure() {
         ErrorCode e = m_candle.setLEDs(50, 50, 2);
         return handleErrorCode(e);
     }
+
+    public boolean signalTrapNotWorking(){
+        ErrorCode e = m_candle.setLEDs(255, 0, 0);
+        return handleErrorCode(e);
+    }
+
+    public boolean signalTrapRunning(){
+        ErrorCode e = m_candle.setLEDs(17, 0, 255);
+        return handleErrorCode(e);
+    }
+
+    public boolean signalTrapFinished(){
+        ErrorCode e = m_candle.setLEDs(0, 255, 0);
+        return handleErrorCode(e);
+    }
 
     private boolean handleErrorCode(ErrorCode e) {
         if (e.equals(ErrorCode.OK)) {