diff --git a/.idea/misc.xml b/.idea/misc.xml index ba9cfe8..360e6d4 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,7 +1,7 @@ - + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckHunter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckHunter.java new file mode 100644 index 0000000..04e07ec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckHunter.java @@ -0,0 +1,375 @@ +/* Copyright (c) 2020 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import android.graphics.Bitmap; +import android.graphics.Color; +import android.graphics.ImageFormat; +import android.os.Build; +import android.os.Handler; + +import androidx.annotation.NonNull; +import androidx.annotation.RequiresApi; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.android.util.Size; +import org.firstinspires.ftc.robotcore.external.function.Consumer; +import org.firstinspires.ftc.robotcore.external.function.Continuation; +import org.firstinspires.ftc.robotcore.external.hardware.camera.Camera; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCaptureRequest; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCaptureSequenceId; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCaptureSession; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCharacteristics; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraException; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraFrame; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraManager; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.internal.collections.EvictingBlockingQueue; +import org.firstinspires.ftc.robotcore.internal.network.CallbackLooper; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.robotcore.internal.system.ContinuationSynchronizer; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.io.File; +import java.io.FileOutputStream; +import java.io.IOException; +import java.util.Locale; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.TimeUnit; + +/** + * This OpMode illustrates how to open a webcam and retrieve images from it. It requires a configuration + * containing a webcam with the default name ("Webcam 1"). When the opmode runs, pressing the 'A' button + * will cause a frame from the camera to be written to a file on the device, which can then be retrieved + * by various means (e.g.: Device File Explorer in Android Studio; plugging the device into a PC and + * using Media Transfer; ADB; etc) + */ +@TeleOp(name="DuckHunter", group ="FTC") +@Disabled +public class DuckHunter extends LinearOpMode { + + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + private static final String TAG = "DuckHunter"; + + /** How long we are to wait to be granted permission to use the camera before giving up. Here, + * we wait indefinitely */ + private static final int secondsPermissionTimeout = Integer.MAX_VALUE; + + /** State regarding our interaction with the camera */ + private CameraManager cameraManager; + private WebcamName cameraName; + private Camera camera; + private CameraCaptureSession cameraCaptureSession; + + /** The queue into which all frames from the camera are placed as they become available. + * Frames which are not processed by the OpMode are automatically discarded. */ + private EvictingBlockingQueue frameQueue; + + /** State regarding where and how to save frames when the 'A' button is pressed. */ + private int captureCounter = 0; + private File captureDirectory = AppUtil.ROBOT_DATA_DIR; + + /** A utility object that indicates where the asynchronous callbacks from the camera + * infrastructure are to run. In this OpMode, that's all hidden from you (but see {@link #startCamera} + * if you're curious): no knowledge of multi-threading is needed here. */ + private Handler callbackHandler; + + /** + * Coordinates for the position of the first, second and third possible duck location + */ + private int[] leftmost = {200, 400}; + private int[] middle = {800, 1000}; + private int[] rightmost = { 1400, 1600}; + + /** + * Hue range for color detection + */ + private int[] hue = {50, 65}; + + //---------------------------------------------------------------------------------------------- + // Main OpMode entry + //---------------------------------------------------------------------------------------------- + + @RequiresApi(api = Build.VERSION_CODES.Q) + @Override public void runOpMode() { + + callbackHandler = CallbackLooper.getDefault().getHandler(); + + cameraManager = ClassFactory.getInstance().getCameraManager(); + cameraName = hardwareMap.get(WebcamName.class, "Webcam 1"); + + initializeFrameQueue(2); + AppUtil.getInstance().ensureDirectoryExists(captureDirectory); + + try { + openCamera(); + if (camera == null) return; + + startCamera(); + if (cameraCaptureSession == null) return; + + telemetry.addData(">", "Press Play to start"); + telemetry.update(); + waitForStart(); + telemetry.clear(); + telemetry.addData(">", "Started...Press 'A' to capture frame"); + + boolean buttonPressSeen = false; + boolean captureWhenAvailable = false; + while (opModeIsActive()) { + + boolean buttonIsPressed = gamepad1.a; + if (buttonIsPressed && !buttonPressSeen) { + captureWhenAvailable = true; + } + buttonPressSeen = buttonIsPressed; + + if (captureWhenAvailable) { + Bitmap bmp = frameQueue.poll(); + if (bmp != null) { + captureWhenAvailable = false; + onNewFrame(bmp); + } + } + + telemetry.update(); + } + } finally { + closeCamera(); + } + } + + /** Do something with the frame */ + @RequiresApi(api = Build.VERSION_CODES.Q) + private void onNewFrame(Bitmap frame) { + saveBitmap(frame); + int YellowInLeftmostSection = 0; + int YellowInMiddleSection = 0; + int YellowInRightmostSection = 0; + + for (int i = leftmost[0]; i <= leftmost[1]; i++) { + for (int j = 0; j < frame.getHeight(); j++) { + Color pixel_color = frame.getColor(i, j); + int rgb = pixel_color.toArgb(); //get RGB value from Color class + float hsv[] = new float[3]; + Color.RGBToHSV(Color.red(Color.red(rgb)), Color.green(rgb), Color.blue(rgb), hsv); //Convert to hsv for easier comparison + if (hsv[0] >= 50 && hsv[0] <= 65) { + YellowInLeftmostSection++; + } + } + } + + for (int i = middle[0]; i <= middle[1]; i++) { + for (int j = 0; j < frame.getHeight(); j++) { + Color pixel_color = frame.getColor(i, j); + int rgb = pixel_color.toArgb(); //get RGB value from Color class + float hsv[] = new float[3]; + Color.RGBToHSV(Color.red(Color.red(rgb)), Color.green(rgb), Color.blue(rgb), hsv); //Convert to hsv for easier comparison + if (hsv[0] >= hue[0] && hsv[0] <= hue[1]) { + YellowInMiddleSection++; + } + } + } + + for (int i = rightmost[0]; i <= rightmost[1]; i++) { + for (int j = 0; j < frame.getHeight(); j++) { + Color pixel_color = frame.getColor(i, j); + int rgb = pixel_color.toArgb(); //get RGB value from Color class + float hsv[] = new float[3]; + Color.RGBToHSV(Color.red(Color.red(rgb)), Color.green(rgb), Color.blue(rgb), hsv); //Convert to hsv for easier comparison + if (hsv[0] >= hue[0] && hsv[0] <= hue[1]) { + YellowInRightmostSection++; + } + } + } + + if (YellowInLeftmostSection > YellowInMiddleSection && YellowInLeftmostSection > YellowInRightmostSection){ + // TODO: Add function call for duck in leftmost position + } else if (YellowInMiddleSection > YellowInRightmostSection){ + // TODO: Add function call for duck in middle position + } else{ + // TODO: Add function call for duck in rightmost position + } + + frame.recycle(); // not strictly necessary, but helpful + } + + //---------------------------------------------------------------------------------------------- + // Camera operations + //---------------------------------------------------------------------------------------------- + + private void initializeFrameQueue(int capacity) { + /** The frame queue will automatically throw away bitmap frames if they are not processed + * quickly by the OpMode. This avoids a buildup of frames in memory */ + frameQueue = new EvictingBlockingQueue(new ArrayBlockingQueue(capacity)); + frameQueue.setEvictAction(new Consumer() { + @Override public void accept(Bitmap frame) { + // RobotLog.ii(TAG, "frame recycled w/o processing"); + frame.recycle(); // not strictly necessary, but helpful + } + }); + } + + private void openCamera() { + if (camera != null) return; // be idempotent + + Deadline deadline = new Deadline(secondsPermissionTimeout, TimeUnit.SECONDS); + camera = cameraManager.requestPermissionAndOpenCamera(deadline, cameraName, null); + if (camera == null) { + error("camera not found or permission to use not granted: %s", cameraName); + } + } + + private void startCamera() { + if (cameraCaptureSession != null) return; // be idempotent + + /** YUY2 is supported by all Webcams, per the USB Webcam standard: See "USB Device Class Definition + * for Video Devices: Uncompressed Payload, Table 2-1". Further, often this is the *only* + * image format supported by a camera */ + final int imageFormat = ImageFormat.YUY2; + + /** Verify that the image is supported, and fetch size and desired frame rate if so */ + CameraCharacteristics cameraCharacteristics = cameraName.getCameraCharacteristics(); + if (!contains(cameraCharacteristics.getAndroidFormats(), imageFormat)) { + error("image format not supported"); + return; + } + final Size size = cameraCharacteristics.getDefaultSize(imageFormat); + final int fps = cameraCharacteristics.getMaxFramesPerSecond(imageFormat, size); + + /** Some of the logic below runs asynchronously on other threads. Use of the synchronizer + * here allows us to wait in this method until all that asynchrony completes before returning. */ + final ContinuationSynchronizer synchronizer = new ContinuationSynchronizer<>(); + try { + /** Create a session in which requests to capture frames can be made */ + camera.createCaptureSession(Continuation.create(callbackHandler, new CameraCaptureSession.StateCallbackDefault() { + @Override public void onConfigured(@NonNull CameraCaptureSession session) { + try { + /** The session is ready to go. Start requesting frames */ + final CameraCaptureRequest captureRequest = camera.createCaptureRequest(imageFormat, size, fps); + session.startCapture(captureRequest, + new CameraCaptureSession.CaptureCallback() { + @Override public void onNewFrame(@NonNull CameraCaptureSession session, @NonNull CameraCaptureRequest request, @NonNull CameraFrame cameraFrame) { + /** A new frame is available. The frame data has not been copied for us, and we can only access it + * for the duration of the callback. So we copy here manually. */ + Bitmap bmp = captureRequest.createEmptyBitmap(); + cameraFrame.copyToBitmap(bmp); + frameQueue.offer(bmp); + } + }, + Continuation.create(callbackHandler, new CameraCaptureSession.StatusCallback() { + @Override public void onCaptureSequenceCompleted(@NonNull CameraCaptureSession session, CameraCaptureSequenceId cameraCaptureSequenceId, long lastFrameNumber) { + RobotLog.ii(TAG, "capture sequence %s reports completed: lastFrame=%d", cameraCaptureSequenceId, lastFrameNumber); + } + }) + ); + synchronizer.finish(session); + } catch (CameraException|RuntimeException e) { + RobotLog.ee(TAG, e, "exception starting capture"); + error("exception starting capture"); + session.close(); + synchronizer.finish(null); + } + } + })); + } catch (CameraException|RuntimeException e) { + RobotLog.ee(TAG, e, "exception starting camera"); + error("exception starting camera"); + synchronizer.finish(null); + } + + /** Wait for all the asynchrony to complete */ + try { + synchronizer.await(); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + + /** Retrieve the created session. This will be null on error. */ + cameraCaptureSession = synchronizer.getValue(); + } + + private void stopCamera() { + if (cameraCaptureSession != null) { + cameraCaptureSession.stopCapture(); + cameraCaptureSession.close(); + cameraCaptureSession = null; + } + } + + private void closeCamera() { + stopCamera(); + if (camera != null) { + camera.close(); + camera = null; + } + } + + //---------------------------------------------------------------------------------------------- + // Utilities + //---------------------------------------------------------------------------------------------- + + private void error(String msg) { + telemetry.log().add(msg); + telemetry.update(); + } + private void error(String format, Object...args) { + telemetry.log().add(format, args); + telemetry.update(); + } + + private boolean contains(int[] array, int value) { + for (int i : array) { + if (i == value) return true; + } + return false; + } + + private void saveBitmap(Bitmap bitmap) { + File file = new File(captureDirectory, String.format(Locale.getDefault(), "webcam-frame-%d.jpg", captureCounter++)); + try { + try (FileOutputStream outputStream = new FileOutputStream(file)) { + bitmap.compress(Bitmap.CompressFormat.JPEG, 100, outputStream); + telemetry.log().add("captured %s", file.getName()); + } + } catch (IOException e) { + RobotLog.ee(TAG, e, "exception in saveBitmap()"); + error("exception saving %s", file.getName()); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cam_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cam_test.java index d8121da..7936d14 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cam_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cam_test.java @@ -101,7 +101,7 @@ public class cam_test extends LinearOpMode { @Override public void runOpMode() { // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that - // first. + // fiConceptWebcamrst. initVuforia(); initTfod();