fix ToggleCameraFeed command

[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")
  private CameraFeeds(/* Joystick Button */) {

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

  }

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

  }

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

  public void toggleCamera() {
    System.out.println("enter toggleCamer");
    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) {
    System.out.println("change camera");
    server.removeCamera(curCam.getName());
    server.addCamera(cam);
  }

  public String getCurrentCamName() {
    return curCam.getName();
  }
}