Working apriltag positioning logic!

TeamCode/src/main/java/opmodes/AutoBase.java

@@ -1,5 +1,6 @@
 package opmodes;
 
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
@@ -62,7 +63,6 @@ public abstract class AutoBase extends LinearOpMode {
         // If the prop is visible at this point, then it must be in the center (2) position
         determinePropLocation();
 
-        TrajectorySequenceBuilder builder;
         switch (this.propLocation) {
             case Left:
                 propLeft();
@@ -75,7 +75,7 @@ public abstract class AutoBase extends LinearOpMode {
                 propRight();
                 break;
         }
-        moveToBackstage();
+        moveToEasel();
         prepareToScore();
         scorePreloadedPixel();
         park();
@@ -108,7 +108,6 @@ public abstract class AutoBase extends LinearOpMode {
         this.robot.getGantry().stop();
         this.robot.getGantry().setSlideTarget(0);
         this.robot.getGantry().armInSync();
-
     }
 
     protected void prepareToScore() {
@@ -129,30 +128,51 @@ public abstract class AutoBase extends LinearOpMode {
                 delta = APRIL_TAG_RIGHT_DELTA;
                 break;
         }
-        double distance = this.robot.getCamera().getDistanceToAprilTag(this.alliance == CenterStageCommon.Alliance.Blue ? 2:5, 25, 5);
+
-        Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() +
+        Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
-                (distance - SCORING_DISTANCE_FROM_APRIL_TAG),
+        this.robot.getDrive().setPoseEstimate(inferredPos);
-                this.robot.getDrive().getPoseEstimate().getY() + delta);
+        Pose2d target = new Pose2d(
+                60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag
+                inferredPos.getY() + delta,
+                0);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(target);
+        builder.lineToLinearHeading(target);
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
-    protected void moveToBackstage() {
+    protected void moveToEasel() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
 
+        if (!this.isBackstage()) {
+            if (this.alliance == CenterStageCommon.Alliance.Blue) {
+                builder.lineToSplineHeading(new Pose2d(-40, 60, Math.PI));
+                builder.lineToLinearHeading(new Pose2d(12, 60, Math.PI));
+            } else if (this.alliance == CenterStageCommon.Alliance.Red) {
+                builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI));
+                builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI));
+            }
+        }
+
         if (this.alliance == CenterStageCommon.Alliance.Blue) {
-            builder.lineToLinearHeading(new Pose2d(35, 11, 0));
+            builder.lineToLinearHeading(new Pose2d(35, 36, 0));
-            builder.lineToLinearHeading(new Pose2d(35, 38, 0));
+        } else if (this.alliance == CenterStageCommon.Alliance.Red) {
-        } else {
+            builder.lineToLinearHeading(new Pose2d(35, -35, 0));
-            builder.lineToLinearHeading(new Pose2d(35, -11, 0));
-            builder.lineToLinearHeading(new Pose2d(35, -36, 0));
         }
 
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
     protected void determinePropLocation() {
+        this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN);
+
+        while (!this.robot.getClaw().isArmAtPosition()) {
+            this.robot.update();
+            sleep(20);
+        }
+
+        sleep(250);
+
         setPropLocationIfVisible(Center, Unknown);
         if (this.propLocation != Center) {
             peekRight();
@@ -160,11 +180,15 @@ public abstract class AutoBase extends LinearOpMode {
     }
 
     protected void peekRight() {
-        TrajectorySequenceBuilder builder = this.robot.getDrive()
+        Pose2d currentPose = this.robot.getDrive().getPoseEstimate();
-                .trajectorySequenceBuilder(initialPosition);
+        final double y = currentPose.getY() > 0 ? -5 : 5;
-        builder.forward(5);
+        final double z = Math.toRadians(-25);
-        builder.turn(Math.toRadians(-33));
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(currentPose.plus(new Pose2d(0, y, z)));
         this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        this.sleep(250);
+
         setPropLocationIfVisible(Right, Left);
     }
 
@@ -188,7 +212,11 @@ public abstract class AutoBase extends LinearOpMode {
         double currentX = this.robot.getDrive().getPoseEstimate().getX();
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.strafeTo(new Vector2d(currentX, park.getY()));
-        builder.lineToConstantHeading(park.vec());
+        builder.lineToLinearHeading(park);
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
+
+    protected boolean isBackstage() {
+        return this.initialPosition.getX() > 0;
+    }
 }

TeamCode/src/main/java/opmodes/BlueBackStage.java

@@ -11,48 +11,44 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
 public class BlueBackStage extends AutoBase {
-    private final Pose2d rendezvous = new Pose2d(12, 10);
+    private static final int delay = 0;
 
     public BlueBackStage() {
         super(
                 CenterStageCommon.Alliance.Blue,
-                new Pose2d(12, 63, Math.toRadians(-90)),
+                new Pose2d(12, 63, Math.toRadians(90)),
                 new Pose2d(62, 62));
     }
 
     protected void propLeft() {
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        this.sleep(delay);
-        builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
-        this.robot.getDrive().followTrajectorySequence(builder.build());
 
-        openAndLiftClaw();
-    }
-
-    protected void propCenter() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(rendezvous.vec());
+        builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
 
         builder = this.robot.getTrajectorySequenceBuilder();
-        builder.turn(Math.toRadians(90));
+        builder.lineToConstantHeading(new Vector2d(24, 50));
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
-    protected void propRight() {
+    protected void propCenter() {
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
+        builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading()));
-        builder.lineToConstantHeading(new Vector2d(19, 34));
+        this.robot.getDrive().followTrajectorySequence(builder.build());
-        builder.addTemporalMarker(0.5, () -> {
+
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        openAndLiftClaw();
-        });
+    }
+
+    protected void propRight() {
+        this.sleep(delay);
+
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0)));
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();

TeamCode/src/main/java/opmodes/BlueFrontStage.java

@@ -11,59 +11,42 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
 public class BlueFrontStage extends AutoBase {
-    private final Pose2d rendezvous = new Pose2d(-36, 11);
+    private static final int delay = 0;
 
     public BlueFrontStage() {
         super(
                 CenterStageCommon.Alliance.Blue,
-                new Pose2d(-36, 63, Math.toRadians(-90)),
+                new Pose2d(-36, 63, Math.toRadians(90)),
-                new Pose2d(62, 12));
+                new Pose2d(62, 12, Math.toRadians(0)));
     }
 
     protected void propLeft() {
-        this.sleep(5000);
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
+        builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180)));
-        builder.lineToConstantHeading(new Vector2d(-40, 31));
-        builder.addTemporalMarker(0.2, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
-
-        builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(rendezvous);
-        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
+
     protected void propCenter() {
-        this.sleep(5000);
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(rendezvous.vec());
+        builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading()));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
-
-        builder = this.robot.getTrajectorySequenceBuilder();
-        builder.turn(Math.toRadians(90));
-        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
+
     protected void propRight() {
-        this.sleep(5000);
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123)));
+        builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
-
-        builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(this.rendezvous);
-        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 }

TeamCode/src/main/java/opmodes/MainTeleOp.java

@@ -1,18 +1,18 @@
 package opmodes;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_UP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
 
 import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -40,10 +40,12 @@ public class MainTeleOp extends OpMode {
     @Override
     public void init() {
         this.dashboard = FtcDashboard.getInstance();
+        this.telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
         this.clawArmPosition = PICKUP_ARM_MAX;
 
         this.robot = new Robot(this.hardwareMap, telemetry);
         this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue);
+        this.robot.getDrive().setPoseEstimate(new Pose2d(12, 63, Math.toRadians(90)));
         telemetry.addData("Status", "Initialized");
     }
 
@@ -170,7 +172,7 @@ public class MainTeleOp extends OpMode {
             this.robot.getLift().stopReset();
         }
 
-        Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
+        Pose2d poseFromAprilTag = this.robot.getCamera().estimatePoseFromAprilTag();
         dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
         dashboard.getTelemetry().update();
 
@@ -223,12 +225,11 @@ public class MainTeleOp extends OpMode {
         }
 
         Pose2d target;  // defines a new pose2d named target, position not yet given
-        Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), -Math.toRadians(poseFromAprilTag.getHeading()));
+        double y = poseFromAprilTag.getY() > 0
-        double y = poseEstimate.getY() > 0
                 ? left ? 40 : 30
                 : left ? -30 : -40;
-        this.robot.getDrive().setPoseEstimate(poseEstimate);
+        this.robot.getDrive().setPoseEstimate(poseFromAprilTag);
-        target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0);
+        target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, y - CAMERA_SIDE_OFFSET_IN, 0);
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(target);
         this.robot.getDrive().followTrajectorySequenceAsync(builder.build());

TeamCode/src/main/java/opmodes/RedBackStage.java

@@ -11,52 +11,47 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
 public class RedBackStage extends AutoBase {
-    private final Pose2d rendezvous = new Pose2d(12, -11);
+    private static final int delay = 0;
 
     public RedBackStage() {
         super(
                 CenterStageCommon.Alliance.Red,
-                new Pose2d(12, -63, Math.toRadians(90)),
+                new Pose2d(12, -63, Math.toRadians(-90)),
                 new Pose2d(62, -62));
     }
 
     protected void propLeft() {
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(32, -34, Math.toRadians(0)));
+        builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0)));
-        builder.lineToConstantHeading(new Vector2d(19, -34));
-        builder.addTemporalMarker(0.5, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
     }
 
     protected void propCenter() {
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(rendezvous.vec());
+        builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading()));
-        builder.addDisplacementMarker(10, () -> {
+        this.robot.getDrive().followTrajectorySequence(builder.build());
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+
-        });
+        openAndLiftClaw();
+    }
+
+    protected void propRight() {
+        this.sleep(delay);
+
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15)));
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
 
         builder = this.robot.getTrajectorySequenceBuilder();
-        builder.turn(Math.toRadians(-90));
+        builder.lineToConstantHeading(new Vector2d(24, -50));
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
-
-    protected void propRight() {
-        // red back side
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(33)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
-        this.robot.getDrive().followTrajectorySequence(builder.build());
-
-        openAndLiftClaw();
-    }
 }
 

TeamCode/src/main/java/opmodes/RedFrontStage.java

@@ -11,63 +11,42 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp")
 public class RedFrontStage extends AutoBase {
-    private final Pose2d rendezvous = new Pose2d(-36, -10);
+    private static final int delay = 0;
 
     public RedFrontStage() {
         super(
                 CenterStageCommon.Alliance.Red,
-                new Pose2d(-36, -63, Math.toRadians(90)),
+                new Pose2d(-36, -63, Math.toRadians(-90)),
                 new Pose2d(61, -12));
     }
 
-    // propLeft will be a reverse of BlueFrontpropRight
     protected void propLeft() {
-        this.sleep(5000);
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(-58, -17, Math.toRadians(123)));
+        builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
-
-        builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(this.rendezvous);
-        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
     protected void propCenter() {
-        this.sleep(5000);
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(rendezvous.vec());
+        builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading()));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
-
-        builder = this.robot.getTrajectorySequenceBuilder();
-        builder.turn(Math.toRadians(-90));
-        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
-    // propRight will be the reverse of BlueFrontpropRight
     protected void propRight() {
-        this.sleep(5000);
+        this.sleep(delay);
+
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180)));
+        builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180)));
-        builder.lineToConstantHeading(new Vector2d(-42, -31));
-        builder.addTemporalMarker(0.5, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
-
-        builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(this.rendezvous);
-        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 }

TeamCode/src/main/java/opmodes/Test.java

@@ -1,20 +1,17 @@
 package opmodes;
 
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.hardware.Camera;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
-import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
-import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
 @TeleOp(name = "Test", group = "Main")
 public class Test extends OpMode {
@@ -41,7 +38,7 @@ public class Test extends OpMode {
         boolean slowmode = gamepad1.right_bumper || gamepad1.y;
         this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
 
-        Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
+        Pose2d poseFromAprilTag = this.robot.getCamera().estimatePoseFromAprilTag();
         dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
         dashboard.getTelemetry().update();
 
@@ -96,7 +93,7 @@ public class Test extends OpMode {
                 ? left ? 40 : 30
                 : left ? -30 : -40;
         this.robot.getDrive().setPoseEstimate(poseEstimate);
-        target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0);
+        target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, y - CAMERA_SIDE_OFFSET_IN, 0);
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(target);
         this.robot.getDrive().followTrajectorySequenceAsync(builder.build());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java

@@ -1,19 +1,24 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_ROTATION_DEG;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_MINI_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME;
 import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
 
+import static java.lang.Math.cos;
+import static java.lang.Math.sin;
+import static java.lang.Math.tan;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.apache.commons.math3.ml.neuralnet.twod.util.TopographicErrorHistogram;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
 import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 import org.firstinspires.ftc.teamcode.vision.Detection;
 import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
@@ -22,12 +27,14 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
 
-import java.util.Arrays;
+import java.util.List;
+
+import lombok.NonNull;
 
 @Config
 public class Camera {
-    public static float PROP_REJECTION_VERTICAL_UPPER = 10;
+    public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
-    public static float PROP_REJECTION_VERTICAL_LOWER = 470;
+    public static float PROP_REJECTION_VERTICAL_LOWER = 440;
     public static float PROP_REJECTION_HORIZONTAL_LEFT = 10;
     public static float PROP_REJECTION_HORIZONTAL_RIGHT = 630;
     private PropDetectionPipeline prop;
@@ -91,27 +98,13 @@ public class Camera {
         return INVALID_DETECTION;
     }
 
-    public AprilTagDetection getAprilTag(int ... ids) {
+    public AprilTagDetection getAprilTag() {
         return this.aprilTag.getDetections()
                 .stream()
-                .filter(x -> Arrays.stream(ids).filter(id -> x.id == id).count() > 0)
                 .findFirst()
                 .orElse(null);
     }
 
-    public double getDistanceToAprilTag(int id, double rejectAbove, double rejectBelow) {
-        for (int i = 0; i < 10; i++) {
-            AprilTagDetection aprilTagDetection = getAprilTag(id);
-            if (aprilTagDetection != null) {
-                if (aprilTagDetection.ftcPose.y < rejectAbove
-                        && aprilTagDetection.ftcPose.y > rejectBelow) {
-                    return aprilTagDetection.ftcPose.y;
-                }
-            }
-        }
-        return Double.MAX_VALUE;
-    }
-
     public void setAlliance(CenterStageCommon.Alliance alliance) {
         this.prop.setAlliance(alliance);
     }
@@ -120,36 +113,71 @@ public class Camera {
         return this.prop != null ? this.prop.getAlliance() : null;
     }
 
-    public Pose2d getPoseFromAprilTag(int ... ids) {
+    public Pose2d estimatePoseFromAprilTag() {
-        if (ids == null || ids.length == 0) {
+        List<AprilTagDetection> aprilTagDetections = aprilTag.getDetections();
-            ids = new int[]{2, 5};
-        }
 
-        AprilTagDetection aprilTagDetection = getAprilTag(ids);
+        if (aprilTagDetections == null || aprilTagDetections.isEmpty()) {
-
-        if (aprilTagDetection == null) {
             return null;
         }
 
-        AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose;
+//        return estimatePoseFromAprilTag(aprilTagDetections.get(0));
 
-        double ourPoseX;
+        int numDetections = aprilTagDetections.size();
-        double ourPoseY;
+        Pose2d acc = new Pose2d(0, 0, 0);
-        switch (aprilTagDetection.id) {
+        for (AprilTagDetection aprilTagDetection : aprilTagDetections) {
-            case 2:
+            acc = acc.plus(estimatePoseFromAprilTag(aprilTagDetection));
-                ourPoseX = tag2Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
-                ourPoseY = tag2Pose.getY() + ftcPose.x - SIDE_OFFSET_IN;
-                break;
-            case 5:
-                ourPoseX = tag5Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
-                ourPoseY = tag5Pose.getY() + ftcPose.x - SIDE_OFFSET_IN;
-                break;
-            default:
-                ourPoseX = 0;
-                ourPoseY = 0;
-                break;
         }
 
-        return new Pose2d(ourPoseX, ourPoseY, ftcPose.bearing);
+        return acc.div(numDetections);
+    }
+
+    private Pose2d estimatePoseFromAprilTag(@NonNull AprilTagDetection aprilTagDetection) {
+        VectorF reference = aprilTagDetection.metadata.fieldPosition;
+        AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose;
+
+        double ax = reference.get(0);
+        double ay = reference.get(1);
+        double t = -Math.toRadians(ftcPose.yaw);
+        double r = -ftcPose.range;
+        double b = Math.toRadians(ftcPose.bearing);
+
+        double x1 = r * cos(b);
+        double y1 = r * sin(b);
+
+        double x2 = x1 * cos(t) - y1 * sin(t);
+        double y2 = x1 * sin(t) + y1 * cos(t);
+
+        double cx = ax + x2;
+        double cy = ay + y2;
+
+        double t1 = t + Math.toRadians(CAMERA_ROTATION_DEG);
+        double h = tan(t1);
+
+        double rx = -CAMERA_FORWARD_OFFSET_IN * cos(t1) + CAMERA_SIDE_OFFSET_IN * sin(t1);
+        double ry = -CAMERA_FORWARD_OFFSET_IN * sin(t1) - CAMERA_SIDE_OFFSET_IN * cos(t1);
+
+        rx += cx;
+        ry += cy;
+
+//        AprilTagDetection foo = aprilTagDetection;
+//        telemetry.addData("id", foo.id);
+//        telemetry.addData("ax", foo.metadata.fieldPosition.get(0));
+//        telemetry.addData("ay", foo.metadata.fieldPosition.get(1));
+//        telemetry.addData("yaw", foo.ftcPose.yaw);
+//        telemetry.addData("range", foo.ftcPose.range);
+//        telemetry.addData("bearing", foo.ftcPose.bearing);
+//        telemetry.addData("x1", x1);
+//        telemetry.addData("y1", y1);
+//        telemetry.addData("x2", x2);
+//        telemetry.addData("y2", y2);
+//        telemetry.addData("cx", cx);
+//        telemetry.addData("cy", cy);
+//        telemetry.addData("t1", t1);
+//        telemetry.addData("h", h);
+//        telemetry.addData("rx", rx);
+//        telemetry.addData("ry", ry);
+//        telemetry.update();
+
+        return new Pose2d(rx, ry, h);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_KP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
@@ -22,6 +23,8 @@ public class Claw implements Updatable {
     private Telemetry telemetry;
     PController clawController = new PController(CLAW_KP);
     private double clawControllerTarget;
+    PController armController = new PController(CLAW_ARM_KP);
+    private double armControllerTarget = -1;
 
     public Claw(HardwareMap hardwareMap) {
         this.claw = hardwareMap.get(Servo.class, CLAW_NAME);
@@ -58,15 +61,32 @@ public class Claw implements Updatable {
         this.armRight.setPosition(target);
     }
 
+    public void setArmPositionAsync(double armControllerTarget) {
+        this.armControllerTarget = armControllerTarget;
+    }
+
+    public boolean isArmAtPosition() {
+        return this.armController.atSetPoint();
+    }
     public void update() {
         this.clawController.setSetPoint(this.clawControllerTarget);
         this.clawController.setTolerance(0.001);
         this.clawController.setP(CLAW_KP);
 
-        double output = 0;
+        this.armController.setSetPoint(this.armControllerTarget);
+        this.armController.setP(CLAW_ARM_KP);
+
         if (!this.clawController.atSetPoint()) {
+            double output = 0;
             output = Math.max(-1 * CLAW_MAX, Math.min(CLAW_MAX, this.clawController.calculate(claw.getPosition())));
             this.claw.setPosition(this.claw.getPosition() + output);
         }
+
+        if (this.armControllerTarget > 0 && !this.armController.atSetPoint()) {
+            double output = 0;
+            output = Math.max(-1 * PICKUP_ARM_MAX, Math.min(PICKUP_ARM_MAX, this.armController.calculate(armLeft.getPosition())));
+            this.armLeft.setPosition(this.armLeft.getPosition() + output);
+            this.armRight.setPosition(this.armRight.getPosition() + output);
+        }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java

@@ -32,12 +32,13 @@ public class RobotConfig {
     public static double AUTO_STRAFE_SLOWDOWN = 4;
 
     // Arm
-    public static double PICKUP_ARM_MIN = 0.185;
+    public static double PICKUP_ARM_MIN = 0.175;
     public static double PICKUP_ARM_MAX = 0.760;
     public static double CLAW_MIN = 0.89;
     public static double CLAW_MAX = 0.65;
     public static double CLAW_ARM_DELTA = 0.03;
     public static double CLAW_KP = 0.3;
+    public static double CLAW_ARM_KP = 0.15;
 
     // Gantry
     public static double GANTRY_ARM_MIN = 0.42;
@@ -68,7 +69,8 @@ public class RobotConfig {
     // Vision
     public static double DETECTION_AREA_MIN = 0.05f;
     public static double DETECTION_AREA_MAX = 0.8f;
-    public static double SCORING_DISTANCE_FROM_APRIL_TAG = 7f;
+    public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6.25f;
-    public static final double FORWARD_OFFSET_IN = 7.75;
+    public static final double CAMERA_FORWARD_OFFSET_IN = 7.77;
-    public static final double SIDE_OFFSET_IN = 0.5;
+    public static final double CAMERA_SIDE_OFFSET_IN = 0.707;
+    public static final double CAMERA_ROTATION_DEG = 0;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java

@@ -74,9 +74,9 @@ public class DriveConstants {
      * Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
      */
     public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
-            RevHubOrientationOnRobot.LogoFacingDirection.UP;
+            RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
     public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
-            RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+            RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
 
 
     public static double encoderTicksToInches(double ticks) {