add code for switching b/w cameras

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

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);
  }
}