Camera auto
This commit is contained in:
parent
aa4509b1f9
commit
6d1582c26a
|
@ -0,0 +1,89 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||||
|
import org.firstinspires.ftc.teamcode.createdcode.enhancedautos.vendor.AprilTagDetectionPipeline;
|
||||||
|
import org.openftc.apriltag.AprilTagDetection;
|
||||||
|
import org.openftc.easyopencv.OpenCvCamera;
|
||||||
|
import org.openftc.easyopencv.OpenCvCameraFactory;
|
||||||
|
import org.openftc.easyopencv.OpenCvCameraRotation;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
|
||||||
|
@Autonomous
|
||||||
|
public class AutoCameraRight extends LinearOpMode {
|
||||||
|
OpenCvCamera camera;
|
||||||
|
AprilTagDetectionPipeline aprilTagDetectionPipeline;
|
||||||
|
|
||||||
|
int detected_id = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
|
||||||
|
"cameraMonitorViewId",
|
||||||
|
"id",
|
||||||
|
hardwareMap.appContext.getPackageName());
|
||||||
|
camera = OpenCvCameraFactory.getInstance().createWebcam(
|
||||||
|
hardwareMap.get(WebcamName.class, "Webcam 1"));
|
||||||
|
aprilTagDetectionPipeline = new AprilTagDetectionPipeline(
|
||||||
|
CameraConstants.TAGSIZE,
|
||||||
|
CameraConstants.FX,
|
||||||
|
CameraConstants.FY,
|
||||||
|
CameraConstants.CX,
|
||||||
|
CameraConstants.CY);
|
||||||
|
API api = new API(this);
|
||||||
|
MovementAPI movementAPI = new MovementAPI(api);
|
||||||
|
|
||||||
|
camera.setPipeline(aprilTagDetectionPipeline);
|
||||||
|
camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
|
||||||
|
@Override
|
||||||
|
public void onOpened() {
|
||||||
|
camera.startStreaming(800, 448, OpenCvCameraRotation.UPRIGHT);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void onError(int errorCode) {
|
||||||
|
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
// let the camera settle
|
||||||
|
api.pause(5);
|
||||||
|
|
||||||
|
ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();
|
||||||
|
if (currentDetections.size() != 0) {
|
||||||
|
detected_id = currentDetections.get(currentDetections.size() - 1).id;
|
||||||
|
telemetry.addData("Tag found", String.valueOf(detected_id));
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (detected_id) {
|
||||||
|
case 1:
|
||||||
|
movementAPI.move(90, 0.7);
|
||||||
|
api.pause(1);
|
||||||
|
movementAPI.move(0, 0.5);
|
||||||
|
api.pause(0.7);
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
movementAPI.move(0, 0.5);
|
||||||
|
api.pause(0.7);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
movementAPI.move(-90, 0.7);
|
||||||
|
api.pause(1);
|
||||||
|
movementAPI.move(0, 0.5);
|
||||||
|
api.pause(0.7);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
movementAPI.move(90, 0.5);
|
||||||
|
api.pause(0.75);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
movementAPI.stop();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,14 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
|
||||||
|
|
||||||
|
public class CameraConstants {
|
||||||
|
// Lens intrinsics
|
||||||
|
// UNITS ARE PIXELS
|
||||||
|
// NOTE: this calibration is for the C920 webcam at 800x448.
|
||||||
|
// You will need to do your own calibration for other configurations!
|
||||||
|
public static final double FX = 578.272;
|
||||||
|
public static final double FY = 578.272;
|
||||||
|
public static final double CX = 402.145;
|
||||||
|
public static final double CY = 221.506;
|
||||||
|
|
||||||
|
public static final double TAGSIZE = 0.166;
|
||||||
|
}
|
|
@ -0,0 +1,63 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||||
|
import org.firstinspires.ftc.teamcode.createdcode.enhancedautos.vendor.AprilTagDetectionPipeline;
|
||||||
|
import org.openftc.apriltag.AprilTagDetection;
|
||||||
|
import org.openftc.easyopencv.OpenCvCamera;
|
||||||
|
import org.openftc.easyopencv.OpenCvCameraFactory;
|
||||||
|
import org.openftc.easyopencv.OpenCvCameraRotation;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
|
||||||
|
@Autonomous
|
||||||
|
public class CameraTest extends LinearOpMode {
|
||||||
|
OpenCvCamera camera;
|
||||||
|
AprilTagDetectionPipeline aprilTagDetectionPipeline;
|
||||||
|
|
||||||
|
// meters
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
|
||||||
|
camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
|
||||||
|
aprilTagDetectionPipeline = new AprilTagDetectionPipeline(
|
||||||
|
CameraConstants.TAGSIZE,
|
||||||
|
CameraConstants.FX,
|
||||||
|
CameraConstants.FY,
|
||||||
|
CameraConstants.CX,
|
||||||
|
CameraConstants.CY);
|
||||||
|
|
||||||
|
camera.setPipeline(aprilTagDetectionPipeline);
|
||||||
|
camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
|
||||||
|
@Override
|
||||||
|
public void onOpened() {
|
||||||
|
camera.startStreaming(800, 448, OpenCvCameraRotation.UPRIGHT);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void onError(int errorCode) {
|
||||||
|
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();
|
||||||
|
|
||||||
|
if (currentDetections.size() != 0) {
|
||||||
|
for (AprilTagDetection tag : currentDetections) {
|
||||||
|
telemetry.addLine(String.format("Detected tag ID=%d", tag.id));
|
||||||
|
}
|
||||||
|
telemetry.update();
|
||||||
|
} else {
|
||||||
|
telemetry.addLine("No tag detected");
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
Loading…
Reference in a new issue