eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working dual cameras.

This commit is contained in:
Scott Barnes 2024-01-20 15:56:15 -06:00
parent 4858eabe16
commit 1a38bec605
5 changed files with 45 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -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;

4
TeamCode/src/main/java/opmodes/Test.java
View File

@ -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;
}

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

@ -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);
}
}

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

@ -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;

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java
View File

@ -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);