diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 672b82f..73e1ed8 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -19,6 +19,7 @@ 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; @TeleOp(name = "MainTeleOp", group = "Main") public class MainTeleOp extends OpMode { @@ -42,6 +43,7 @@ public class MainTeleOp extends OpMode { this.clawArmPosition = PICKUP_ARM_MAX; this.robot = new Robot(this.hardwareMap, telemetry); + this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue); telemetry.addData("Status", "Initialized"); } @@ -168,7 +170,7 @@ public class MainTeleOp extends OpMode { this.robot.getLift().stopReset(); } - Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); dashboard.getTelemetry().update(); @@ -215,13 +217,13 @@ public class MainTeleOp extends OpMode { this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); } - private void macroToScore(Vector2d poseFromAprilTag, boolean left) { + private void macroToScore(Pose2d poseFromAprilTag, boolean left) { if (this.robot.getDrive().isBusy()) { return; } Pose2d target; // defines a new pose2d named target, position not yet given - Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading()); + Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), -Math.toRadians(poseFromAprilTag.getHeading())); double y = poseEstimate.getY() > 0 ? left ? 40 : 30 : left ? -30 : -40; diff --git a/TeamCode/src/main/java/opmodes/Test.java b/TeamCode/src/main/java/opmodes/Test.java index 606fa14..a9fe231 100644 --- a/TeamCode/src/main/java/opmodes/Test.java +++ b/TeamCode/src/main/java/opmodes/Test.java @@ -41,7 +41,7 @@ public class Test extends OpMode { boolean slowmode = gamepad1.right_bumper || gamepad1.y; this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode); - Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); dashboard.getTelemetry().update(); @@ -86,7 +86,7 @@ public class Test extends OpMode { this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); } - private void macroToScore(Vector2d poseFromAprilTag, boolean left) { + private void macroToScore(Pose2d poseFromAprilTag, boolean left) { if (this.robot.getDrive().isBusy()) { return; } 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 d36bdee..dc57d1b 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 @@ -2,21 +2,18 @@ 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.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 android.hardware.GeomagneticField; - -import androidx.annotation.NonNull; - 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.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer; import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import org.firstinspires.ftc.teamcode.vision.Detection; import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline; @@ -24,19 +21,19 @@ import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.jetbrains.annotations.NotNull; import java.util.Arrays; @Config public class Camera { - public static float PROP_REJECTION_VERTICAL_UPPER = 150; - public static float PROP_REJECTION_VERTICAL_LOWER = 275; - public static float PROP_REJECTION_HORIZONTAL_LEFT = 50; - public static float PROP_REJECTION_HORIZONTAL_RIGHT = 480 - PROP_REJECTION_HORIZONTAL_LEFT; + public static float PROP_REJECTION_VERTICAL_UPPER = 10; + public static float PROP_REJECTION_VERTICAL_LOWER = 470; + public static float PROP_REJECTION_HORIZONTAL_LEFT = 10; + public static float PROP_REJECTION_HORIZONTAL_RIGHT = 630; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; - private VisionPortal visionPortal; + private VisionPortal aprilTagPortal; + private VisionPortal propPortal; private Telemetry telemetry; public static final Vector2d tag2Pose = new Vector2d(60, 36); public static final Vector2d tag5Pose = new Vector2d(60, -36); @@ -54,13 +51,30 @@ public class Camera { .setDrawTagID(true) .setDrawTagOutline(true) .build(); + int[] viewportIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL); + + VisionPortal.Builder aprilTagVisionPortalBuilder = new VisionPortal.Builder(); + aprilTagVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, WEBCAM_NAME)); + aprilTagVisionPortalBuilder.setLiveViewContainerId(viewportIds[0]); + aprilTagVisionPortalBuilder.setAutoStopLiveView(true); + aprilTagVisionPortalBuilder.addProcessor(aprilTag); + this.aprilTagPortal = aprilTagVisionPortalBuilder.build(); + this.prop = new PropDetectionPipeline(); - this.visionPortal = VisionPortal.easyCreateWithDefaults( - hardwareMap.get(WebcamName.class, WEBCAM_NAME), aprilTag, prop); + VisionPortal.Builder propVisionPortalBuilder = new VisionPortal.Builder(); + propVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, WEBCAM_MINI_NAME)); + propVisionPortalBuilder.setLiveViewContainerId(viewportIds[1]); + propVisionPortalBuilder.setAutoStopLiveView(true); + propVisionPortalBuilder.addProcessor(prop); + this.propPortal = propVisionPortalBuilder.build(); + + this.propPortal.resumeLiveView(); + this.aprilTagPortal.resumeLiveView(); this.initialized = true; } public Detection getProp() { + telemetry.addData("Getting Prop - Alliance", this.getAlliance()); if (!initialized || getAlliance() == null) { return INVALID_DETECTION; } @@ -106,7 +120,7 @@ public class Camera { return this.prop != null ? this.prop.getAlliance() : null; } - public Vector2d getPoseFromAprilTag(int ... ids) { + public Pose2d getPoseFromAprilTag(int ... ids) { if (ids == null || ids.length == 0) { ids = new int[]{2, 5}; } @@ -136,6 +150,6 @@ public class Camera { break; } - return new Vector2d(ourPoseX, ourPoseY); + return new Pose2d(ourPoseX, ourPoseY, ftcPose.bearing); } } \ No newline at end of file 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 5b6d54c..2677689 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 @@ -20,6 +20,7 @@ public class RobotConfig { public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation"; public static final String ROBOT_LIFT_LIFT_NAME = "liftLift"; public static final String WEBCAM_NAME = "webcam"; + public static final String WEBCAM_MINI_NAME = "webcammini"; public static final String PARALLEL_DRIVE_ODOMETRY = FRONT_LEFT_NAME; public static final String PERPENDICULAR_DRIVE_ODOMETRY = BACK_LEFT_NAME; @@ -65,12 +66,8 @@ public class RobotConfig { public static double LIFT_POWER = 1f; // Vision - public static double CAMERA_OFFSET_X = 0f; - public static double DETECTION_AREA_MIN = 0.01f; - public static double DETECTION_AREA_MAX = 0.2f; - public static double DETECTION_LEFT_X = 0; - public static double DETECTION_CENTER_X = .5; - public static double DETECTION_RIGHT_X = 1; + 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 final double FORWARD_OFFSET_IN = 7.75; public static final double SIDE_OFFSET_IN = 0.5; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java index 90ca238..f044ead 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java @@ -6,8 +6,6 @@ import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERT import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_UPPER; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MIN; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_LEFT_X; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGHT_X; import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE; import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1; import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2; @@ -23,8 +21,6 @@ import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour; import android.graphics.Canvas; import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; -import org.firstinspires.ftc.teamcode.hardware.Camera; -import org.firstinspires.ftc.teamcode.hardware.Gantry; import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import org.firstinspires.ftc.teamcode.util.ScalarRange; import org.firstinspires.ftc.vision.VisionProcessor; @@ -39,7 +35,6 @@ import java.util.ArrayList; import lombok.Getter; import lombok.Setter; -import opmodes.Test; public class PropDetectionPipeline implements VisionProcessor { @Getter @@ -113,13 +108,14 @@ public class PropDetectionPipeline implements VisionProcessor { @Override public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { - canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE); - canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER * scaleBmpPxToCanvasPx, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER * scaleBmpPxToCanvasPx, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER * scaleBmpPxToCanvasPx, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER * scaleBmpPxToCanvasPx, WHITE); + canvas.drawLine(PROP_REJECTION_HORIZONTAL_LEFT * scaleBmpPxToCanvasPx, 0, PROP_REJECTION_HORIZONTAL_LEFT * scaleBmpPxToCanvasPx, canvas.getHeight(), WHITE); + canvas.drawLine(PROP_REJECTION_HORIZONTAL_RIGHT * scaleBmpPxToCanvasPx, 0, PROP_REJECTION_HORIZONTAL_RIGHT * scaleBmpPxToCanvasPx, canvas.getHeight(), WHITE); if (red != null && red.isValid()) { Point center = red.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL_LOWER - && center.y > PROP_REJECTION_VERTICAL_UPPER) { + if (center.y < PROP_REJECTION_VERTICAL_LOWER && center.y > PROP_REJECTION_VERTICAL_UPPER) { canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE); } else { canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, RED); @@ -128,8 +124,7 @@ public class PropDetectionPipeline implements VisionProcessor { if (blue != null && blue.isValid()) { Point center = blue.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL_LOWER - && center.y > PROP_REJECTION_VERTICAL_UPPER) { + if (center.y < PROP_REJECTION_VERTICAL_LOWER && center.y > PROP_REJECTION_VERTICAL_UPPER) { canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); } else { canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, RED);