import com.ni.vision.VisionException; //导入依赖的package包/类
public synchronized void openCamera() {
  if (m_id != -1)
    return; // Camera is already open
  for (int i = 0; i < 3; i++) {
    try {
      m_id =
          NIVision.IMAQdxOpenCamera(m_name,
              NIVision.IMAQdxCameraControlMode.CameraControlModeController);
    } catch (VisionException e) {
      if (i == 2)
        throw e;
      delay(2.0);
      continue;
    }
    break;
  }
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
private void initCamera() {
try {
    session = NIVision.IMAQdxOpenCamera("cam0", NIVision.IMAQdxCameraControlMode.CameraControlModeController);
    NIVision.IMAQdxConfigureGrab(session);
  //  NIVision.IMAQdx

    // create images
    image = NIVision.imaqCreateImage(ImageType.IMAGE_RGB, 0);

    // Put default values to SmartDashboard so fields will appear
    SmartDashboard.putBoolean("use binary", false);
} catch (VisionException e) {
    TurtleLogger.critical("Camera Not found");
    e.printStackTrace();
}

   }
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * Manually change the image that is served by the MJPEG stream. This can be
 * called to pass custom annotated images to the dashboard. Note that, for
 * 640x480 video, this method could take between 40 and 50 milliseconds to
 * complete.
 *
 * This shouldn't be called if {@link #startAutomaticCapture} is called.
 *
 * @param image The IMAQ image to show on the dashboard
 */
public void setImage(Image image) {
  // handle multi-threadedness

  /* Flatten the IMAQ image to a JPEG */
  RawData data =
      NIVision.imaqFlatten(image, NIVision.FlattenType.FLATTEN_IMAGE,
          NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality);
  ByteBuffer buffer = data.getBuffer();
  boolean hwClient;

  synchronized (this) {
    hwClient = m_hwClient;
  }

  /* Find the start of the JPEG data */
  int index = 0;
  if (hwClient) {
    while (index < buffer.limit() - 1) {
      if ((buffer.get(index) & 0xff) == 0xFF && (buffer.get(index + 1) & 0xff) == 0xD8)
        break;
      index++;
    }
  }

  if (buffer.limit() - index - 1 <= 2) {
    throw new VisionException("data size of flattened image is less than 2. Try another camera! ");
  }

  setImageData(data, index);
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * Start automatically capturing images to send to the dashboard.
 *
 * You should call this method to just see a camera feed on the dashboard
 * without doing any vision processing on the roboRIO. {@link #setImage}
 * shouldn't be called after this is called.
 *
 * @param cameraName The name of the camera interface (e.g. "cam1")
 */
public void startAutomaticCapture(String cameraName) {
  try {
    USBCamera camera = new USBCamera(cameraName);
    camera.openCamera();
    startAutomaticCapture(camera);
  } catch (VisionException ex) {
    //DriverStation.reportError(
       // "Error when starting the camera: " + cameraName + " " + ex.getMessage(), true);
  }
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
protected void capture() {
  Image frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
  while (true) {
    boolean hwClient;
    ByteBuffer dataBuffer = null;
    synchronized (this) {
      hwClient = m_hwClient;
      if (hwClient) {
        dataBuffer = m_imageDataPool.removeLast();
      }
    }

    try {
      if (hwClient && dataBuffer != null) {
        // Reset the image buffer limit
        dataBuffer.limit(dataBuffer.capacity() - 1);
        m_camera[selectedCamera].getImageData(dataBuffer);
        setImageData(new RawData(dataBuffer), 0);
      } else {
        m_camera[selectedCamera].getImage(frame);
        setImage(frame);
      }
    } catch (VisionException ex) {
      //DriverStation.reportError("Error when getting image from the camera: " + ex.getMessage(),true);
      if (dataBuffer != null) {
        synchronized (this) {
          m_imageDataPool.addLast(dataBuffer);
          Timer.delay(.1);
        }
      }
    }
  }
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
public void setSelectedCamera(int id) {
 if (lastSwitch + 2500 > System.currentTimeMillis())
  return;
 if (selectedCamera != id && id >= 0 && id < m_camera.length) {
  try {
	  m_camera[selectedCamera].stopCapture();
	  m_camera[id].startCapture();
	  
	  selectedCamera = id;
  } catch (VisionException e) {
	  //DriverStation.reportError("A camera was unplugged.", true);
  }
  lastSwitch = System.currentTimeMillis();
 }
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * Manually change the image that is served by the MJPEG stream. This can be
 * called to pass custom annotated images to the dashboard. Note that, for
 * 640x480 video, this method could take between 40 and 50 milliseconds to
 * complete.
 *
 * 
 *
 * @param image The IMAQ image to show on the dashboard
 */
public void setImage(Image image) {
  // handle multi-threadedness

  /* Flatten the IMAQ image to a JPEG */

  RawData data =
      NIVision.imaqFlatten(image, NIVision.FlattenType.FLATTEN_IMAGE,
          NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality);
  ByteBuffer buffer = data.getBuffer();
  boolean hwClient;

  synchronized (this) {
    hwClient = m_hwClient;
  }

  /* Find the start of the JPEG data */
  int index = 0;
  if (hwClient) {
    while (index < buffer.limit() - 1) {
      if ((buffer.get(index) & 0xff) == 0xFF && (buffer.get(index + 1) & 0xff) == 0xD8)
        break;
      index++;
    }
  }

  if (buffer.limit() - index - 1 <= 2) {
    throw new VisionException("data size of flattened image is less than 2. Try another camera! ");
  }

  setImageData(data, index);
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * Start automatically capturing images to send to the dashboard.
 *
 * You should call this method to just see a camera feed on the dashboard
 * without doing any vision processing on the roboRIO. {@link #setImage}
 * shouldn't be called after this is called.
 *
 * @param cameraName The name of the camera interface (e.g. "cam1")
 */
private USBCamera startCamera(String cameraName) {
  try {
    USBCamera camera = new USBCamera(cameraName);
    camera.openCamera();
    //startAutomaticCapture(camera);
    return camera;
  } catch (VisionException ex) {
    DriverStation.reportError(
        "Error when starting the camera: " + cameraName + " " + ex.getMessage(), true);
  }
  return null;
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
protected void capture() {
  Image frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
  while (true) {
    boolean hwClient;
    ByteBuffer dataBuffer = null;
    synchronized (this) {
      hwClient = m_hwClient;
      if (hwClient) {
        dataBuffer = m_imageDataPool.removeLast();
      }
    }

    try {
      if (hwClient && dataBuffer != null) {
        // Reset the image buffer limit
        dataBuffer.limit(dataBuffer.capacity() - 1);
        m_camera.getImageData(dataBuffer);
        setImageData(new RawData(dataBuffer), 0);
      } else {
        m_camera.getImage(frame);
        setImage(frame);
      }
    } catch (VisionException ex) {
      DriverStation.reportError("Error when getting image from the camera: " + ex.getMessage(),
          true);
      if (dataBuffer != null) {
        synchronized (this) {
          m_imageDataPool.addLast(dataBuffer);
          Timer.delay(.1);
        }
      }
    }
  }
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * Manually change the image that is served by the MJPEG stream. This can be
 * called to pass custom annotated images to the dashboard. Note that, for
 * 640x480 video, this method could take between 40 and 50 milliseconds to
 * complete.
 *
 * This shouldn't be called if {@link #startAutomaticCapture} is called.
 *
 * @param image The IMAQ image to show on the dashboard
 */
public void setImage(Image image) {
  // handle multi-threadedness

  /* Flatten the IMAQ image to a JPEG */

  RawData data =
      NIVision.imaqFlatten(image, NIVision.FlattenType.FLATTEN_IMAGE,
          NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality);
  ByteBuffer buffer = data.getBuffer();
  boolean hwClient;

  synchronized (this) {
    hwClient = m_hwClient;
  }

  /* Find the start of the JPEG data */
  int index = 0;
  if (hwClient) {
    while (index < buffer.limit() - 1) {
      if ((buffer.get(index) & 0xff) == 0xFF && (buffer.get(index + 1) & 0xff) == 0xD8)
        break;
      index++;
    }
  }

  if (buffer.limit() - index - 1 <= 2) {
    throw new VisionException("data size of flattened image is less than 2. Try another camera! ");
  }

  setImageData(data, index);
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * Start automatically capturing images to send to the dashboard.
 *
 * You should call this method to just see a camera feed on the dashboard
 * without doing any vision processing on the roboRIO. {@link #setImage}
 * shouldn't be called after this is called.
 *
 * @param cameraName The name of the camera interface (e.g. "cam1")
 */
public void startAutomaticCapture(String cameraName) {
  try {
    USBCamera camera = new USBCamera(cameraName);
    camera.openCamera();
    startAutomaticCapture(camera);
  } catch (VisionException ex) {
    DriverStation.reportError(
        "Error when starting the camera: " + cameraName + " " + ex.getMessage(), true);
  }
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
@Override
protected void execute() {
	try{
		Robot.cameraSubsystem.serveLatestFrame();
	}catch(VisionException e){
		SmartDashboard.putString("Vision Error", e.getMessage()+e.getStackTrace());
	}
	
	String selectionString = (String)cameraChooser.getSelected();
	boolean selection = false;
	if (selectionString=="cam2"){
		selection = true;
	}
	
	/*if (selection!=Robot.cameraSubsystem.getInitilizedCamera()){
		Robot.cameraSubsystem.selectCamera(selection);
		Robot.cameraSubsystem.serveLatestFrame();
	}*/
	
	if(Robot.oi.joystick1.getRawButton(4) || Robot.oi.joystick2.getRawButton(4))
	{
		Robot.cameraSubsystem.selectCamera(true);
	}
	else if(Robot.oi.joystick1.getRawButton(5) || Robot.oi.joystick2.getRawButton(5))
	{
		Robot.cameraSubsystem.selectCamera(false);
	}
		
		
	SmartDashboard.putString("Target XY", SmartDashboard.getString("DIAMOND_COORDINATES", "?"));
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
private void resetAcquisition(){
	if (!broken){
		try{
			NIVision.IMAQdxStopAcquisition(getCurrentSession());
			NIVision.IMAQdxUnconfigureAcquisition(getCurrentSession());
		}catch (VisionException v){broken=true;pushError(v);}
	}
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
public void selectCamera(boolean id){
	if (!broken){
		try{
			if (doneInit) resetAcquisition();
			NIVision.IMAQdxConfigureGrab(getSession(id));
	        NIVision.IMAQdxStartAcquisition(getSession(id));
	        initilizedCamera = id;
	        doneInit=true;
		}catch (VisionException v){broken=true;pushError(v);}
	}
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
public void serveLatestFrame(){
	if(!broken){
		try{
			NIVision.IMAQdxGrab(getCurrentSession(), frame, 1);
			server.setImage(frame);
		}catch (VisionException v){broken=true;pushError(v);}
	}
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
	try {
		session = NIVision.IMAQdxOpenCamera("cam0", NIVision.IMAQdxCameraControlMode.CameraControlModeController);
		NIVision.IMAQdxConfigureGrab(session);
		image = NIVision.imaqCreateImage(ImageType.IMAGE_RGB, 0);
	} catch (VisionException e) {

	}

}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
@Override
public void initialize() {
	try {
		frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);  // create the image frame for cam
		session = NIVision.IMAQdxOpenCamera("cam0",
		        NIVision.IMAQdxCameraControlMode.CameraControlModeController);  // get reference to camera
		NIVision.IMAQdxConfigureGrab(session);  // grab current streaming session
	} catch (VisionException e) {
		e.printStackTrace();
	}
}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	public void robotInit() {
		oi = new OI();
		driveTrainEncoder.initEncoders();
		// autonomous chooser
		chooser = new SendableChooser();
		chooser.addDefault("Position One", "Position One");
		chooser.addObject("Position Two", "Position Two");
		chooser.addObject("Position Three", "Position Three");
		chooser.addObject("Position Four", "Position Four");
		chooser.addObject("Position Five", "Position Five");
		chooser.addObject("Do Nothing", "Do Nothing");
		SmartDashboard.putData("Auto mode", chooser);
		SmartDashboard.putData("LoadPrefNames", new LoadPrefNames());
		// camera chooser
		cameraSelector = new SendableChooser();
		cameraSelector.addDefault("Front View", "Front View");
		cameraSelector.addObject("Back View", "Back View");
		cameraSelector.addObject("Manual Change", "Manual Change");
		SmartDashboard.putData("Camera Selector", cameraSelector);
		
		
		
		try {
			frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
//			sessionfront = NIVision.IMAQdxOpenCamera("cam3",
//					NIVision.IMAQdxCameraControlMode.CameraControlModeController);
			sessionfront = NIVision.IMAQdxOpenCamera("cam0",
					NIVision.IMAQdxCameraControlMode.CameraControlModeController);
			sessionback = NIVision.IMAQdxOpenCamera("cam2",
					NIVision.IMAQdxCameraControlMode.CameraControlModeController);
			currSession = sessionback;
			NIVision.IMAQdxConfigureGrab(currSession);
		} catch (VisionException v) {
			//do nothing
		} catch (Exception e) {
			
		}
		// drivetrain chooser
		drivetrainSelector = new SendableChooser();
		drivetrainSelector.addDefault("Tank Drive", "Tank Drive");
		drivetrainSelector.addObject("Arcade Drive", "Arcade Drive");
		SmartDashboard.putData("Drivetrain Selector", drivetrainSelector);
		// preferences
		prefs = Preferences.getInstance();
		distanceTarget = prefs.getDouble("DistanceTarget", distanceTarget);
		distanceOffset = prefs.getDouble("DistanceOffset", distanceOffset);
		angleMultiplier = prefs.getDouble("AngleMultipler", angleMultiplier);
		angleAddition = prefs.getDouble("AngleAddition", angleAddition);
		distanceMultiplier = prefs.getDouble("DistanceMultipler",
				distanceMultiplier);
		distanceAddition = prefs
				.getDouble("DistanceAddition", distanceAddition);
		leftInches = prefs.getDouble("lWheelInches", leftInches);
		rightInches = prefs.getDouble("fRightInches", rightInches);
		leftTicks = prefs.getDouble("lWheelTicks", leftTicks);
		rightTicks = prefs.getDouble("rWheelTicks", rightTicks);
		// add sensors
		LiveWindow.addSensor("Sensors", "Gyro", gyro);
	}
 

import com.ni.vision.VisionException; //导入依赖的package包/类
private void pushError(VisionException v){
	SmartDashboard.putString("VisionError", v.getMessage()+" -- "+v.toString());
}