From 821b3b12e914f23dc8afedd71088327beb600842 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Thu, 25 Jan 2024 23:05:14 -0600 Subject: [PATCH] Drone launch pause --- TeamCode/src/main/java/opmodes/MainTeleOp.java | 13 ++++++++++--- .../firstinspires/ftc/teamcode/hardware/Camera.java | 9 ++------- .../firstinspires/ftc/teamcode/hardware/Drone.java | 5 +++++ .../ftc/teamcode/hardware/RobotConfig.java | 5 +++-- 4 files changed, 20 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index f2bbaee..96ca9e1 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/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()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index 86e1a0d..38e6a5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -120,13 +120,7 @@ public class Camera { return null; } - int numDetections = aprilTagDetections.size(); - Pose2d acc = new Pose2d(0, 0, 0); - for (AprilTagDetection aprilTagDetection : aprilTagDetections) { - acc = acc.plus(estimatePoseFromAprilTag(aprilTagDetection)); - } - - return acc.div(numDetections); + return estimatePoseFromAprilTag(aprilTagDetections.stream().max((a, b) -> (int)(a.decisionMargin - b.decisionMargin)).get()); } 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java index 22820fd..921449c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java +++ b/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); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index 9c56d8c..4c85a12 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java +++ b/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;