Fix small bug in toggle.

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / CameraFeeds.java

package org.usfirst.frc.team3501.robot;

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 VideoSource curCam;
  private CameraServer server;
  private static UsbCamera intakeCam;
  private static AxisCamera climberCam;
  private static String usbCamName;
  private static String axisCamName;
  private static CameraFeeds cameraFeeds = null;

  @SuppressWarnings("deprecation")
  public CameraFeeds(/* Joystick Button */) {
    // Get camera id by supplying camera name example '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; // Image that will contain camera image frame =
     * NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0); // Server that
     * we'll give the image to server = CameraServer.getInstance();
     * server.setSize(Constants.CameraFeeds.imgQuality);
     */

    server = CameraServer.getInstance();
    climberCam = server.addAxisCamera("axisCamera", "10.35.1.11");
    intakeCam = server.startAutomaticCapture();
    curCam = intakeCam;
    usbCamName = intakeCam.getName();
    axisCamName = climberCam.getName();

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

  public static CameraFeeds getCameraFeeds() {
    if (cameraFeeds == null) {
      cameraFeeds = new CameraFeeds();
    }
    return cameraFeeds;

  }

  public void init() {
    changeCam(climberCam);
  }

  public void toggleCamera() {
    if (curCam.equals(intakeCam)) {
      changeCam(climberCam);
      curCam = climberCam;
      System.out.println("Switching to climber camera.");
      return;
    }

    if (curCam.equals(climberCam)) {
      changeCam(intakeCam);
      curCam = intakeCam;
      System.out.println("Switching to intake camera.");
      return;
    }
  }

  /**
   *
   * Change the camera to get image from to a different one
   *
   * newId for camera
   */
  public void changeCam(VideoSource cam) {
    server.removeCamera(axisCamName);
    server.removeCamera(usbCamName);
    server.addCamera(cam);
  }
}