add class for toggling between camera feeds

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..c6cd477
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/CameraFeeds.java
@@ -0,0 +1,71 @@
+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 {
+  private final int intakeCam;
+  private final int climberCam;
+  private int curCam;
+  private Image frame;
+  private CameraServer server;
+
+  public CameraFeeds() {
+    // Get camera ids by supplying camera name ex 'cam0', found on roborio web
+    // interface
+    intakeCam = NIVision.IMAQdxOpenCamera(Config.CameraFeeds.camNameCenter,
+        NIVision.IMAQdxCameraControlMode.CameraControlModeController);
+    climberCam = NIVision.IMAQdxOpenCamera(Config.CameraFeeds.camNameRight,
+        NIVision.IMAQdxCameraControlMode.CameraControlModeController);
+    curCam = intakeCam;
+    // Img that will contain camera img
+    frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
+    // Server that we'll give the img to
+    server = CameraServer.getInstance();
+    server.setQuality(Config.CameraFeeds.imgQuality);
+  }
+
+  public void init() {
+    changeCam(intakeCam);
+  }
+
+  public void run()
+  {
+    if()
+      changeCam(intakeCam);
+
+    if()
+      changeCam(climberCam);
+
+    updateCam();
+  }
+
+  /**
+   * Stop aka close camera stream
+   */
+  public void end() {
+    NIVision.IMAQdxStopAcquisition(curCam);
+  }
+
+  /**
+   * Change the camera to get imgs from to a different one
+   *
+   * @param newId
+   *          for camera
+   */
+  public void changeCam(int newId) {
+    NIVision.IMAQdxStopAcquisition(curCam);
+    NIVision.IMAQdxConfigureGrab(newId);
+    NIVision.IMAQdxStartAcquisition(newId);
+    curCam = newId;
+  }
+
+  /**
+   * Get the img from current camera and give it to the server
+   */
+  public void updateCam() {
+    NIVision.IMAQdxGrab(curCam, frame, 1);
+    server.setImage(frame);
+  }
+}