implement CameraFeeds into robot.java and add todos

[3501/2017steamworks] / 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 {
  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(/*add test for toggle*/)
      changeCam(intakeCam);

    if(/*add test for toggle*/)
      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);
  }
}