add code for switching b/w cameras

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / CameraFeeds.java

diff --git a/src/org/usfirst/frc/team3501/robot/CameraFeeds.java b/src/org/usfirst/frc/team3501/robot/CameraFeeds.java
new file mode 100755 (executable)
index 0000000..3221e83
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/CameraFeeds.java
@@ -0,0 +1,76 @@
+package org.usfirst.frc.team3501.robot;
+
+import com.ni.vision.NIVision;
+import com.ni.vision.NIVision.Image;
+
+import edu.wpi.first.wpilibj.CameraServer;
+
+public class CameraFeeds {
+
+  // camera id's
+  private int camIntake;
+  private int camShooter;
+  private int currentCam;
+
+  private Image frame;
+  private CameraServer server;
+
+  public CameraFeeds() {
+    // get camera id's by giving camera name on roboRio web interface
+    camIntake = NIVision.IMAQdxOpenCamera(Constants.CameraFeeds.camNameIntake,
+        NIVision.IMAQdxCameraControlMode.CameraControlModeController);
+    camShooter = NIVision.IMAQdxOpenCamera(Constants.CameraFeeds.camNameIntake,
+        NIVision.IMAQdxCameraControlMode.CameraControlModeController);
+
+    // img that will contain camera img
+    frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
+
+    // server we will give the image to
+    server = CameraServer.getInstance();
+    server.setQuality(Constants.CameraFeeds.imgQuality);
+
+  }
+
+  public void init() {
+    changeCam(camShooter);
+  }
+
+  public void run() {
+    // change camera based on direction robot is moving in (whatever is front)
+    // (initial front is shooter)
+    if (currentCam != camIntake && Robot.driveTrain.isFlipped())
+      changeCam(camIntake);
+    else if (currentCam != camShooter) {
+      changeCam(camShooter);
+    }
+    updateCam();
+  }
+
+  /***
+   * Close camera stream
+   */
+  public void end() {
+    NIVision.IMAQdxStopAcquisition(currentCam);
+  }
+
+  /***
+   * switch which camera your getting images from
+   *
+   * @param newId
+   *          for camera
+   */
+  public void changeCam(int newId) {
+    NIVision.IMAQdxStopAcquisition(currentCam);
+    NIVision.IMAQdxConfigureGrab(newId);
+    NIVision.IMAQdxStartAcquisition(newId);
+    currentCam = newId;
+  }
+
+  /***
+   * Get img from current camera and give it to server
+   */
+  public void updateCam() {
+    NIVision.IMAQdxGrab(currentCam, frame, 1);
+    server.setImage(frame);
+  }
+}