Second attempt at camera server toggling.

[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.cscore.AxisCamera;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.wpilibj.CameraServer;

public class CameraFeeds {
  // private final int intakeCam;
  // private final int climberCam;
  private Image frame;
  private String curCam = "intake";
  private CameraServer server;
  private static UsbCamera intakeCam;
  private static AxisCamera climberCam;

  @SuppressWarnings("deprecation")
  public CameraFeeds(/* Joystick Button */) {
    // Get camera ids by supplying camera name ex 'cam0', found on roborio web
    // interface
    /*
     * intakeCam =
     * NIVision.IMAQdxOpenCamera(Constants.CameraFeeds.camNameCenter,
     * NIVision.IMAQdxCameraControlMode.CameraControlModeController); climberCam
     * = NIVision.IMAQdxOpenCamera(Constants.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.setSize(Constants.CameraFeeds.imgQuality);
     */
    intakeCam = CameraServer.getInstance().startAutomaticCapture();
    CameraServer.getInstance().startAutomaticCapture();
    server = CameraServer.getInstance();
    climberCam = server.addAxisCamera("axisCamera", "10.35.1.11");

    // server = CameraServer.getInstance();
    // axisCamera = cameraServer2.addAxisCamera("axisCamera", "10.35.1.11");
    // cameraFeeds = new CameraFeeds();
  }

  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 */) {
    if (curCam == "climber") {
      server.removeCamera(/* Axis Camera name */);
      server.addCamera(intakeCam);
      curCam = "intake";
    } else if (curCam == "intake") {
      server.removeCamera(/* Usb Camera name */);
      server.addCamera(climberCam);
      curCam = "climber";
    }
    // 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);
  }
}