Drone launch pause

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

@@ -2,6 +2,7 @@ package opmodes;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_LAUNCH_PAUSE_S;
 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.CAMERA_SIDE_OFFSET_IN;
@@ -36,6 +37,8 @@ public class MainTeleOp extends OpMode {
     private FtcDashboard dashboard;
     private boolean isGantryFlipped = false;
     private int targetTagId;
+    private double droneLaunchStart;
+    private boolean wasRobotDroneLaunch;
 
     @Override
     public void init() {
@@ -140,11 +143,15 @@ public class MainTeleOp extends OpMode {
 
         // Robot Drone
 
-        if (robotDroneLaunch) {
+        if (robotDroneLaunch && !wasRobotDroneLaunch) {
             this.robot.getDrone().raise();
-        } else {
+            this.droneLaunchStart = this.getRuntime();
+        } else if (robotDroneLaunch && this.getRuntime() > this.droneLaunchStart + DRONE_LAUNCH_PAUSE_S){
+            this.robot.getDrone().launch();
+        } else if (!robotDroneLaunch) {
             this.robot.getDrone().reset();
         }
+        this.wasRobotDroneLaunch = robotDroneLaunch;
         // Robot Lift
 
         if (robotLiftRotation || this.liftArmShouldBeUp) {
@@ -213,7 +220,7 @@ public class MainTeleOp extends OpMode {
         double y = targetTagId == 2 ? 36f : -36f;
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(34, y, 0));
+        builder.lineToLinearHeading(new Pose2d(35, y, 0));
         this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
     }
 

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

@@ -120,13 +120,7 @@ public class Camera {
             return null;
         }
 
-        int numDetections = aprilTagDetections.size();
+        return estimatePoseFromAprilTag(aprilTagDetections.stream().max((a,  b) -> (int)(a.decisionMargin - b.decisionMargin)).get());
-        Pose2d acc = new Pose2d(0, 0, 0);
-        for (AprilTagDetection aprilTagDetection : aprilTagDetections) {
-            acc = acc.plus(estimatePoseFromAprilTag(aprilTagDetection));
-        }
-
-        return acc.div(numDetections);
     }
 
     private Pose2d estimatePoseFromAprilTag(@NonNull AprilTagDetection aprilTagDetection) {
@@ -159,6 +153,7 @@ public class Camera {
 
 //        AprilTagDetection foo = aprilTagDetection;
 //        telemetry.addData("id", foo.id);
+//        telemetry.addData("decisionMargin", foo.decisionMargin);
 //        telemetry.addData("ax", foo.metadata.fieldPosition.get(0));
 //        telemetry.addData("ay", foo.metadata.fieldPosition.get(1));
 //        telemetry.addData("yaw", foo.ftcPose.yaw);

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

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_LAUNCH_POS;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_STOW_POS;
 
@@ -16,6 +17,10 @@ public class Drone {
     }
 
     public void raise() {
+        this.droneLaunchServo.setPosition(DRONE_ROTATION_UP);
+    }
+
+    public void launch() {
         this.droneLaunchServo.setPosition(DRONE_LAUNCH_POS);
     }
 

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

@@ -46,7 +46,7 @@ public class RobotConfig {
     public static int GANTRY_LIFT_DELTA = 50;
     public static double GANTRY_ARM_KP = 0.1;
     public static double X_KP = 0.1;
-    public static double X_MAX = 0.84;
+    public static double X_MAX = 0.86;
     public static double X_MIN = 0.16;
     public static double X_CENTER = 0.54;
     public static double GANTRY_ARM_DELTA_MAX = 0.013;
@@ -54,9 +54,10 @@ public class RobotConfig {
     public static double SLIDE_POWER = 0.25;
 
     // Robot Drone Launch
-    public static double DRONE_ROTATION_UP = 0.2;
+    public static double DRONE_ROTATION_UP = 0.38;
     public static double DRONE_LAUNCH_POS = 0.5;
     public static double DRONE_STOW_POS = 0.2;
+    public static double DRONE_LAUNCH_PAUSE_S = 0.5;
 
     // Robot Lift
     public static double LIFT_ROTATION_UP = 0.4;