From 338c59e783fd9d4320b5215535f62d098165c68d Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Thu, 7 Nov 2019 09:02:23 -0800
Subject: [PATCH] Start working on USBCamera

---
 Main/chameleon-vision.iml                     | 13 ++++++
 .../camera/CameraProcess.java                 |  2 +-
 .../classabstraction/camera/USBCamera.java    | 40 +++++++++++++++++--
 3 files changed, 51 insertions(+), 4 deletions(-)

diff --git a/Main/chameleon-vision.iml b/Main/chameleon-vision.iml
index 1799ea932..6f10ef5f5 100644
--- a/Main/chameleon-vision.iml
+++ b/Main/chameleon-vision.iml
@@ -12,6 +12,19 @@
     </content>
     <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-java:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxaarch64bionic:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxraspbian:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxx86-64:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:osxx86-64:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:windowsx86-64:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.cameraserver:cameraserver-java:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-java:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:osxx86-64:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:linuxraspbian:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:linuxx86-64:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:windowsx86-64:2019.4.1-213-g56d782b" level="project" />
+    <orderEntry type="library" name="Maven: edu.wpi.first.wpiutil:wpiutil-java:2019.4.1-213-g56d782b" level="project" />
     <orderEntry type="library" name="Maven: io.javalin:javalin:3.4.1" level="project" />
     <orderEntry type="library" name="Maven: org.jetbrains.kotlin:kotlin-stdlib-jdk8:1.3.31" level="project" />
     <orderEntry type="library" name="Maven: org.jetbrains.kotlin:kotlin-stdlib:1.3.31" level="project" />
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/camera/CameraProcess.java b/Main/src/main/java/com/chameleonvision/classabstraction/camera/CameraProcess.java
index fe5777c0c..df4a4c38f 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/camera/CameraProcess.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/camera/CameraProcess.java
@@ -4,7 +4,7 @@ import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
 
 public interface CameraProcess {
-    Pair<Mat, Long> getFrame();
+    Pair<Mat, Long> getFrame(Mat frame);
 
     void setExposure(int exposure);
     void setBrightness(int brightness);
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/camera/USBCamera.java b/Main/src/main/java/com/chameleonvision/classabstraction/camera/USBCamera.java
index 7175ff7f9..15dc844ad 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/camera/USBCamera.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/camera/USBCamera.java
@@ -1,15 +1,49 @@
 package com.chameleonvision.classabstraction.camera;
 
+import edu.wpi.cscore.CvSink;
 import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoException;
 import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.cameraserver.CameraServer;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
 
-public class USBCamera {
+public class USBCamera implements CameraProcess {
     private final UsbCamera baseCamera;
     public final CameraProperties properties;
+    private final CvSink cvSink;
+    private Mat imageBuffer = new Mat();
 
-    public USBCamera(UsbCamera camera) {
+    public USBCamera(UsbCamera camera, double fov) {
         baseCamera = camera;
+        cvSink = CameraServer.getInstance().getVideo(baseCamera);
         VideoMode vidMode = new VideoMode(VideoMode.PixelFormat.kYUYV, 640, 480, 60);
-        properties = new CameraProperties(baseCamera, 75);
+        properties = new CameraProperties(baseCamera, fov);
+    }
+
+    @Override
+    public Pair<Mat, Long> getFrame(Mat frame) {
+        var timestamp = System.nanoTime();
+        cvSink.grabFrame(imageBuffer);
+        imageBuffer.copyTo(frame);
+        return Pair.of(frame, timestamp - System.nanoTime());
+    }
+
+    @Override
+    public void setExposure(int exposure) {
+        try {
+            baseCamera.setExposureManual(exposure);
+        } catch (VideoException e) {
+            System.err.println("USBCamera Does not support exposure change");
+        }
+    }
+
+    @Override
+    public void setBrightness(int brightness) {
+        try {
+            baseCamera.setBrightness(brightness);
+        } catch (VideoException e) {
+            System.err.println("USBCamera Does not support brightness change");
+        }
     }
 }