Don't send DS errors if no DS.

Change-Id: I765d8c21371e772ec09bf696f6dd2fff43f8ed24

.gitignore

@@ -15,6 +15,8 @@ wpilibj/wpilibJavaJNI/nivision/*.cpp
 wpilibj/wpilibJavaJNI/nivision/*.s
 wpilibj/wpilibJavaJNI/nivision/*_arm.ini
 wpilibj/wpilibJavaJNI/nivision/*.java
+wpilibj/wpilibJavaJNI/nivision/nivision_funcs.txt
+wpilibj/wpilibJavaJNI/nivision/imaqdx_funcs.txt
 
 # Created by the jenkins test script
 test-reports

eclipse-plugins/edu.wpi.first.wpilib.plugins.core/src/main/java/edu/wpi/first/wpilib/plugins/core/actions/RunSFXDashboardAction.java

@@ -34,7 +34,7 @@ public class RunSFXDashboardAction implements IWorkbenchWindowActionDelegate {
 	 * @see IWorkbenchWindowActionDelegate#run
 	 */
 	public void run(IAction action) {
-		File dir = new File(WPILibCore.getDefault().getWPILibBaseDir()+File.separator+"tools");
+		File dir = new File(WPILibCore.getDefault().getWPILibBaseDir()+File.separator+"tools"+File.separator+"sfx-2014.11.01");
 		File[] files = dir.listFiles(new FilenameFilter() {
 			@Override public boolean accept(File dir, String name) {
 				return name.startsWith("sfx") && name.endsWith(".jar");

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/.cproject

@@ -81,7 +81,7 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/src&quot;"/>
 									<listOptionValue builtIn="false" value="${WPILIB}/cpp/current/sim/include"/>
 									<listOptionValue builtIn="false" value="/usr/include"/>
-									<listOptionValue builtIn="false" value="/usr/include/gazebo-3.1"/>
+									<listOptionValue builtIn="false" value="/usr/include/gazebo-3.2"/>
 									<listOptionValue builtIn="false" value="/usr/include/sdformat-2.2"/>
 								</option>
 								<option id="gnu.cpp.compiler.option.optimization.level.1648211502" name="Optimization Level" superClass="gnu.cpp.compiler.option.optimization.level" value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated"/>

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Color_src/Robot.cpp

@@ -0,0 +1,237 @@
+#include "WPILib.h"
+#include <vector>
+#include <cmath>
+
+/**
+ * Example of finding yellow totes based on color.
+ * This example utilizes an image file, which you need to copy to the roboRIO
+ * To use a camera you will have to integrate the appropriate camera details with this example.
+ * To use a USB camera instead, see the IntermediateVision example for details
+ * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on
+ * using an Axis Camera.
+ *
+ * Sample images can be found here: http://wp.wpi.edu/wpilib/2015/01/16/sample-images-for-vision-projects/ 
+ */
+class VisionColor2015Sample : public SampleRobot
+{
+	//A structure to hold measurements of a particle
+	struct ParticleReport {
+		double PercentAreaToImageArea;
+		double Area;
+		double ConvexHullArea;
+		double BoundingRectLeft;
+		double BoundingRectTop;
+		double BoundingRectRight;
+		double BoundingRectBottom;
+	};
+
+	//Structure to represent the scores for the various tests used for target identification
+	struct Scores {
+		double Trapezoid;
+		double LongAspect;
+		double ShortAspect;
+		double AreaToConvexHullArea;
+	};
+
+	//Images
+	Image *frame;
+	Image *binaryFrame;
+	int imaqError;
+
+	//Constants
+	Range TOTE_HUE_RANGE = {24, 49};	//Default hue range for yellow tote
+	Range TOTE_SAT_RANGE = {67, 255};	//Default saturation range for yellow tote
+	Range TOTE_VAL_RANGE = {49, 255};	//Default value range for yellow tote
+	double AREA_MINIMUM = 2; //Default Area minimum for particle as a percentage of total image area
+	double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22
+	double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4
+	double SCORE_MIN = 75.0;  //Minimum score to be considered a tote
+	double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480
+	ParticleFilterCriteria2 criteria[1];
+	ParticleFilterOptions2 filterOptions = {0,0,1,1};
+	Scores scores;
+
+
+public:
+	void RobotInit() override {
+	    // create images
+		frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
+		binaryFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0);
+
+		//Put default values to SmartDashboard so fields will appear
+		SmartDashboard::PutNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
+		SmartDashboard::PutNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
+		SmartDashboard::PutNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
+		SmartDashboard::PutNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
+		SmartDashboard::PutNumber("Tote val min", TOTE_VAL_RANGE.minValue);
+		SmartDashboard::PutNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
+		SmartDashboard::PutNumber("Area min %", AREA_MINIMUM);
+	}
+
+	void Autonomous() override {
+		while (IsAutonomous() && IsEnabled())
+		{
+			//read file in from disk. For this example to run you need to copy image20.jpg from the SampleImages folder to the
+			//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
+			imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image20.jpg", NULL, NULL);
+
+			//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
+			TOTE_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
+			TOTE_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
+			TOTE_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
+			TOTE_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
+			TOTE_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", TOTE_VAL_RANGE.minValue);
+			TOTE_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
+
+			//Threshold the image looking for yellow (tote color)
+			imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &TOTE_HUE_RANGE, &TOTE_SAT_RANGE, &TOTE_VAL_RANGE);
+
+			//Send particle count to dashboard
+			int numParticles = 0;
+			imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
+			SmartDashboard::PutNumber("Masked particles", numParticles);
+
+			//Send masked image to dashboard to assist in tweaking mask.
+			SendToDashboard(binaryFrame, imaqError);
+
+			//filter out small particles
+			float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
+			criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
+			imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
+
+			//Send particle count after filtering to dashboard
+			imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
+			SmartDashboard::PutNumber("Filtered particles", numParticles);
+
+			if(numParticles > 0)
+			{
+				//Measure particles and sort by particle size
+				std::vector<ParticleReport> particles;
+				for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
+				{
+					ParticleReport par;
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_CONVEX_HULL_AREA, &(par.ConvexHullArea));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
+					particles.push_back(par);
+				}
+				sort(particles.begin(), particles.end(), CompareParticleSizes);
+
+				//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
+				//for the reader. Note that the long and short side scores expect a single tote and will not work for a stack of 2 or more totes.
+				//Modification of the code to accommodate 2 or more stacked totes is left as an exercise for the reader.
+				scores.Trapezoid = TrapezoidScore(particles.at(0));
+				SmartDashboard::PutNumber("Trapezoid", scores.Trapezoid);
+				scores.LongAspect = LongSideScore(particles.at(0));
+				SmartDashboard::PutNumber("Long Aspect", scores.LongAspect);
+				scores.ShortAspect = ShortSideScore(particles.at(0));
+				SmartDashboard::PutNumber("Short Aspect", scores.ShortAspect);
+				scores.AreaToConvexHullArea = ConvexHullAreaScore(particles.at(0));
+				SmartDashboard::PutNumber("Convex Hull Area", scores.AreaToConvexHullArea);
+				bool isTote = scores.Trapezoid > SCORE_MIN && (scores.LongAspect > SCORE_MIN || scores.ShortAspect > SCORE_MIN) && scores.AreaToConvexHullArea > SCORE_MIN;
+				bool isLong = scores.LongAspect > scores.ShortAspect;
+
+				//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
+				SmartDashboard::PutBoolean("IsTote", isTote);
+				SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0), isLong));
+			} else {
+				SmartDashboard::PutBoolean("IsTote", false);
+			}
+
+			Wait(0.005);				// wait for a motor update time
+		}
+	}
+
+	void OperatorControl() override {
+		while(IsOperatorControl() && IsEnabled()) {
+			Wait(0.005);				// wait for a motor update time
+		}
+	}
+
+
+	//Send image to dashboard if IMAQ has not thrown an error
+	void SendToDashboard(Image *image, int error)
+	{
+		if(error < ERR_SUCCESS) {
+			DriverStation::ReportError("Send To Dashboard error: " + std::to_string((long)imaqError) + "\n");
+		} else {
+			CameraServer::GetInstance()->SetImage(image);
+		}
+	}
+
+	//Comparator function for sorting particles. Returns true if particle 1 is larger
+	static bool CompareParticleSizes(ParticleReport particle1, ParticleReport particle2)
+	{
+		//we want descending sort order
+		return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea;
+	}
+
+	/**
+	 * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
+	 * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
+	 */
+	double ratioToScore(double ratio)
+	{
+		return (fmax(0, fmin(100*(1-fabs(1-ratio)), 100)));
+	}
+
+	/**
+	 * Method to score convex hull area. This scores how "complete" the particle is. Particles with large holes will score worse than a filled in shape
+	 */
+	double ConvexHullAreaScore(ParticleReport report)
+	{
+		return ratioToScore((report.Area/report.ConvexHullArea)*1.18);
+	}
+
+	/**
+	 * Method to score if the particle appears to be a trapezoid. Compares the convex hull (filled in) area to the area of the bounding box.
+	 * The expectation is that the convex hull area is about 95.4% of the bounding box area for an ideal tote.
+	 */
+	double TrapezoidScore(ParticleReport report)
+	{
+		return ratioToScore(report.ConvexHullArea/((report.BoundingRectRight-report.BoundingRectLeft)*(report.BoundingRectBottom-report.BoundingRectTop)*.954));
+	}
+
+	/**
+	 * Method to score if the aspect ratio of the particle appears to match the long side of a tote.
+	 */
+	double LongSideScore(ParticleReport report)
+	{
+		return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))/LONG_RATIO);
+	}
+
+	/**
+	 * Method to score if the aspect ratio of the particle appears to match the short side of a tote.
+	 */
+	double ShortSideScore(ParticleReport report){
+		return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))/SHORT_RATIO);
+	}
+
+	/**
+	 * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics
+	 * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
+	 *
+	 * @param image The image to use for measuring the particle estimated rectangle
+	 * @param report The Particle Analysis Report for the particle
+	 * @param isLong Boolean indicating if the target is believed to be the long side of a tote
+	 * @return The estimated distance to the target in feet.
+	 */
+	double computeDistance (Image *image, ParticleReport report, bool isLong) {
+		double normalizedWidth, targetWidth;
+		int xRes, yRes;
+
+		imaqGetImageSize(image, &xRes, &yRes);
+		normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/xRes;
+		SmartDashboard::PutNumber("Width", normalizedWidth);
+		targetWidth = isLong ? 26.9 : 16.9;
+
+		return  targetWidth/(normalizedWidth*12*tan(VIEW_ANGLE*M_PI/(180*2)));
+	}
+};
+
+START_ROBOT_CLASS(VisionColor2015Sample);
+

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Retro_src/Robot.cpp

@@ -0,0 +1,210 @@
+#include "WPILib.h"
+#include <vector>
+#include <cmath>
+
+/**
+ * Example of finding yellow totes based on retroreflective target.
+ * This example utilizes an image file, which you need to copy to the roboRIO
+ * To use a camera you will have to integrate the appropriate camera details with this example.
+ * To use a USB camera instead, see the IntermediateVision example for details
+ * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on
+ * using an Axis Camera.
+ *
+ * Sample images can be found here: http://wp.wpi.edu/wpilib/2015/01/16/sample-images-for-vision-projects/ 
+ */
+class VisionRetro2015Sample : public SampleRobot
+{
+	//A structure to hold measurements of a particle
+	struct ParticleReport {
+		double PercentAreaToImageArea;
+		double Area;
+		double BoundingRectLeft;
+		double BoundingRectTop;
+		double BoundingRectRight;
+		double BoundingRectBottom;
+	};
+
+	//Structure to represent the scores for the various tests used for target identification
+	struct Scores {
+		double Area;
+		double Aspect;
+	};
+
+	//Images
+	Image *frame;
+	Image *binaryFrame;
+	int imaqError;
+
+	//Constants
+	Range RING_HUE_RANGE = {101, 64};	//Default hue range for ring light
+	Range RING_SAT_RANGE = {88, 255};	//Default saturation range for ring light
+	Range RING_VAL_RANGE = {134, 255};	//Default value range for ring light
+	double AREA_MINIMUM = 0.5; //Default Area minimum for particle as a percentage of total image area
+	double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22
+	double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4
+	double SCORE_MIN = 75.0;  //Minimum score to be considered a tote
+	double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480
+	ParticleFilterCriteria2 criteria[1];
+	ParticleFilterOptions2 filterOptions = {0,0,1,1};
+	Scores scores;
+
+
+public:
+	void RobotInit() override {
+	    // create images
+		frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
+		binaryFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0);
+
+		//Put default values to SmartDashboard so fields will appear
+		SmartDashboard::PutNumber("Tote hue min", RING_HUE_RANGE.minValue);
+		SmartDashboard::PutNumber("Tote hue max", RING_HUE_RANGE.maxValue);
+		SmartDashboard::PutNumber("Tote sat min", RING_SAT_RANGE.minValue);
+		SmartDashboard::PutNumber("Tote sat max", RING_SAT_RANGE.maxValue);
+		SmartDashboard::PutNumber("Tote val min", RING_VAL_RANGE.minValue);
+		SmartDashboard::PutNumber("Tote val max", RING_VAL_RANGE.maxValue);
+		SmartDashboard::PutNumber("Area min %", AREA_MINIMUM);
+	}
+
+	void Autonomous() override {
+		while (IsAutonomous() && IsEnabled())
+		{
+			//read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder to the
+			//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
+			imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image.jpg", NULL, NULL);
+
+			//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
+			RING_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", RING_HUE_RANGE.minValue);
+			RING_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", RING_HUE_RANGE.maxValue);
+			RING_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", RING_SAT_RANGE.minValue);
+			RING_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", RING_SAT_RANGE.maxValue);
+			RING_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", RING_VAL_RANGE.minValue);
+			RING_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", RING_VAL_RANGE.maxValue);
+
+			//Threshold the image looking for ring light color
+			imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &RING_HUE_RANGE, &RING_SAT_RANGE, &RING_VAL_RANGE);
+
+			//Send particle count to dashboard
+			int numParticles = 0;
+			imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
+			SmartDashboard::PutNumber("Masked particles", numParticles);
+
+			//Send masked image to dashboard to assist in tweaking mask.
+			SendToDashboard(binaryFrame, imaqError);
+
+			//filter out small particles
+			float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
+			criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
+			imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
+
+			//Send particle count after filtering to dashboard
+			imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
+			SmartDashboard::PutNumber("Filtered particles", numParticles);
+
+			if(numParticles > 0) {
+				//Measure particles and sort by particle size
+				std::vector<ParticleReport> particles;
+				for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
+				{
+					ParticleReport par;
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
+					imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
+					particles.push_back(par);
+				}
+				sort(particles.begin(), particles.end(), CompareParticleSizes);
+
+				//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
+				//for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information 
+				//about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote.
+				scores.Aspect = AspectScore(particles.at(0));
+				SmartDashboard::PutNumber("Aspect", scores.Aspect);
+				scores.Area = AreaScore(particles.at(0));
+				SmartDashboard::PutNumber("Area", scores.Area);
+				bool isTarget = scores.Area > SCORE_MIN && scores.Aspect > SCORE_MIN;
+
+				//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
+				SmartDashboard::PutBoolean("IsTarget", isTarget);
+				SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0)));
+			} else {
+				SmartDashboard::PutBoolean("IsTarget", false);
+			}
+
+			Wait(0.005);				// wait for a motor update time
+		}
+	}
+
+	void OperatorControl() override {
+		while(IsOperatorControl() && IsEnabled()) {
+			Wait(0.005);				// wait for a motor update time
+		}
+	}
+
+
+	//Send image to dashboard if IMAQ has not thrown an error
+	void SendToDashboard(Image *image, int error)
+	{
+		if(error < ERR_SUCCESS) {
+			DriverStation::ReportError("Send To Dashboard error: " + std::to_string((long)imaqError) + "\n");
+		} else {
+			CameraServer::GetInstance()->SetImage(binaryFrame);
+		}
+	}
+
+	//Comparator function for sorting particles. Returns true if particle 1 is larger
+	static bool CompareParticleSizes(ParticleReport particle1, ParticleReport particle2)
+	{
+		//we want descending sort order
+		return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea;
+	}
+
+	/**
+	 * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
+	 * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
+	 */
+	double ratioToScore(double ratio)
+	{
+		return (fmax(0, fmin(100*(1-fabs(1-ratio)), 100)));
+	}
+
+
+	double AreaScore(ParticleReport report)
+	{
+		double boundingArea = (report.BoundingRectBottom - report.BoundingRectTop) * (report.BoundingRectRight - report.BoundingRectLeft);
+		//Tape is 7" edge so 49" bounding rect. With 2" wide tape it covers 24" of the rect.
+		return ratioToScore((49/24)*report.Area/boundingArea);
+	}
+
+	/**
+	 * Method to score if the aspect ratio of the particle appears to match the retro-reflective target. Target is 7"x7" so aspect should be 1
+	 */
+	double AspectScore(ParticleReport report)
+	{
+		return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop)));
+	}
+
+
+	/**
+	 * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics
+	 * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
+	 *
+	 * @param image The image to use for measuring the particle estimated rectangle
+	 * @param report The Particle Analysis Report for the particle
+	 * @return The estimated distance to the target in feet.
+	 */
+	double computeDistance (Image *image, ParticleReport report) {
+		double normalizedWidth, targetWidth;
+		int xRes, yRes;
+
+		imaqGetImageSize(image, &xRes, &yRes);
+		normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/xRes;
+		SmartDashboard::PutNumber("Width", normalizedWidth);
+		targetWidth = 7;
+
+		return  targetWidth/(normalizedWidth*12*tan(VIEW_ANGLE*M_PI/(180*2)));
+	}
+};
+
+START_ROBOT_CLASS(VisionRetro2015Sample);

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml

@@ -335,7 +335,46 @@
                   destination="src/Robot.cpp"></file>
         </files>
     </example>
+	
+	<example>
+		<name>2015 Vision Color Sample</name>
+        <description>An example program that demonstrates image processing to locate Yellow totes by color.
+			This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing.
+			To use this code with a camera, you must integrate the code for image acquisition from the appropriate
+			camera example;
+        </description>
+        <tags>
+            <tag>Vision</tag>
+            <tag>Complete List</tag>
+        </tags>
+        <packages>
+            <package>src</package>
+        </packages>
+        <files>
+            <file source="examples/2015Vision/Color_src/Robot.cpp"
+                  destination="src/Robot.cpp"></file>
+		</files>
+    </example>
 
+	<example>
+		<name>2015 Vision Retro Sample</name>
+        <description>An example program that demonstrates image processing to locate Yellow totes by the retroreflective target.
+			This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing.
+			To use this code with a camera, you must integrate the code for image acquisition from the appropriate
+			camera example;
+        </description>
+        <tags>
+            <tag>Vision</tag>
+            <tag>Complete List</tag>
+        </tags>
+        <packages>
+            <package>src</package>
+        </packages>
+        <files>
+            <file source="examples/2015Vision/Retro_src/Robot.cpp"
+                  destination="src/Robot.cpp"></file>
+		</files>
+    </example>
 
     <example>
     <name>GearsBot</name>

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/src/main/resources/cpp-zip/ant/build.xml

@@ -64,6 +64,15 @@
 
     <echo>[athena-deploy] Copying code over.</echo>
     <scp file="${out.exe}" sftp="true" todir="${username}@${target}:${deploy.dir}" password="${password}" trust="true"/>
+
+    <sshexec host="${target}"
+               username="admin"
+               password="${password}"
+               trust="true"
+               failonerror="false"
+               command="killall netconsole-host"/>
+    <scp file="${wpilib.ant.dir}/netconsole-host" todir="admin@${target}:/usr/local/frc/bin" password="${password}" trust="true"/>
+
     <scp file="${wpilib.ant.dir}/robotCommand" todir="${username}@${target}:/home/lvuser/" password="${password}" trust="true"/>
 
     <echo>[athena-deploy] Starting program.</echo>

eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Color_src/Robot.java

@@ -0,0 +1,240 @@
+package $package;
+
+import java.lang.Math;
+import java.util.Comparator;
+import java.util.Vector;
+
+import com.ni.vision.NIVision;
+import com.ni.vision.NIVision.Image;
+import com.ni.vision.NIVision.ImageType;
+
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.SampleRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Example of finding yellow totes based on color.
+ * This example utilizes an image file, which you need to copy to the roboRIO
+ * To use a camera you will have to integrate the appropriate camera details with this example.
+ * To use a USB camera instead, see the SimpelVision and AdvancedVision examples for details
+ * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on
+ * using an Axis Camera.
+ *
+ * Sample omages can be found here: http://wp.wpi.edu/wpilib/2015/01/16/sample-images-for-vision-projects/ 
+ */
+public class Robot extends SampleRobot {
+		//A structure to hold measurements of a particle
+		public class ParticleReport implements Comparator<ParticleReport>, Comparable<ParticleReport>{
+			double PercentAreaToImageArea;
+			double Area;
+			double ConvexHullArea;
+			double BoundingRectLeft;
+			double BoundingRectTop;
+			double BoundingRectRight;
+			double BoundingRectBottom;
+			
+			public int compareTo(ParticleReport r)
+			{
+				return (int)(r.Area - this.Area);
+			}
+			
+			public int compare(ParticleReport r1, ParticleReport r2)
+			{
+				return (int)(r1.Area - r2.Area);
+			}
+		};
+
+		//Structure to represent the scores for the various tests used for target identification
+		public class Scores {
+			double Trapezoid;
+			double LongAspect;
+			double ShortAspect;
+			double AreaToConvexHullArea;
+		};
+
+		//Images
+		Image frame;
+		Image binaryFrame;
+		int imaqError;
+
+		//Constants
+		NIVision.Range TOTE_HUE_RANGE = new NIVision.Range(24, 49);	//Default hue range for yellow tote
+		NIVision.Range TOTE_SAT_RANGE = new NIVision.Range(67, 255);	//Default saturation range for yellow tote
+		NIVision.Range TOTE_VAL_RANGE = new NIVision.Range(49, 255);	//Default value range for yellow tote
+		double AREA_MINIMUM = 0.5; //Default Area minimum for particle as a percentage of total image area
+		double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22
+		double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4
+		double SCORE_MIN = 75.0;  //Minimum score to be considered a tote
+		double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480
+		NIVision.ParticleFilterCriteria2 criteria[] = new NIVision.ParticleFilterCriteria2[1];
+		NIVision.ParticleFilterOptions2 filterOptions = new NIVision.ParticleFilterOptions2(0,0,1,1);
+		Scores scores = new Scores();
+
+		public void robotInit() {
+		    // create images
+			frame = NIVision.imaqCreateImage(ImageType.IMAGE_RGB, 0);
+			binaryFrame = NIVision.imaqCreateImage(ImageType.IMAGE_U8, 0);
+			criteria[0] = new NIVision.ParticleFilterCriteria2(NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA, AREA_MINIMUM, 100.0, 0, 0);
+
+			//Put default values to SmartDashboard so fields will appear
+			SmartDashboard.putNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
+			SmartDashboard.putNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
+			SmartDashboard.putNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
+			SmartDashboard.putNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
+			SmartDashboard.putNumber("Tote val min", TOTE_VAL_RANGE.minValue);
+			SmartDashboard.putNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
+			SmartDashboard.putNumber("Area min %", AREA_MINIMUM);
+		}
+
+		public void autonomous() {
+			while (isAutonomous() && isEnabled())
+			{
+				//read file in from disk. For this example to run you need to copy image20.jpg from the SampleImages folder to the
+				//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
+				NIVision.imaqReadFile(frame, "/home/lvuser/SampleImages/image20.jpg");
+
+				//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
+				TOTE_HUE_RANGE.minValue = (int)SmartDashboard.getNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
+				TOTE_HUE_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
+				TOTE_SAT_RANGE.minValue = (int)SmartDashboard.getNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
+				TOTE_SAT_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
+				TOTE_VAL_RANGE.minValue = (int)SmartDashboard.getNumber("Tote val min", TOTE_VAL_RANGE.minValue);
+				TOTE_VAL_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
+
+				//Threshold the image looking for yellow (tote color)
+				NIVision.imaqColorThreshold(binaryFrame, frame, 255, NIVision.ColorMode.HSV, TOTE_HUE_RANGE, TOTE_SAT_RANGE, TOTE_VAL_RANGE);
+
+				//Send particle count to dashboard
+				int numParticles = NIVision.imaqCountParticles(binaryFrame, 1);
+				SmartDashboard.putNumber("Masked particles", numParticles);
+
+				//Send masked image to dashboard to assist in tweaking mask.
+				CameraServer.getInstance().setImage(binaryFrame);
+
+				//filter out small particles
+				float areaMin = (float)SmartDashboard.getNumber("Area min %", AREA_MINIMUM);
+				criteria[0].lower = areaMin;
+				imaqError = NIVision.imaqParticleFilter4(binaryFrame, binaryFrame, criteria, filterOptions, null);
+
+				//Send particle count after filtering to dashboard
+				numParticles = NIVision.imaqCountParticles(binaryFrame, 1);
+				SmartDashboard.putNumber("Filtered particles", numParticles);
+
+				if(numParticles > 0)
+				{
+					//Measure particles and sort by particle size
+					Vector<ParticleReport> particles = new Vector<ParticleReport>();
+					for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
+					{
+						ParticleReport par = new ParticleReport();
+						par.PercentAreaToImageArea = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA);
+						par.Area = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA);
+						par.ConvexHullArea = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_CONVEX_HULL_AREA);
+						par.BoundingRectTop = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_TOP);
+						par.BoundingRectLeft = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_LEFT);
+						par.BoundingRectBottom = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_BOTTOM);
+						par.BoundingRectRight = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_RIGHT);
+						particles.add(par);
+					}
+					particles.sort(null);
+
+					//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
+					//for the reader. Note that the long and short side scores expect a single tote and will not work for a stack of 2 or more totes.
+					//Modification of the code to accommodate 2 or more stacked totes is left as an exercise for the reader.
+					scores.Trapezoid = TrapezoidScore(particles.elementAt(0));
+					SmartDashboard.putNumber("Trapezoid", scores.Trapezoid);
+					scores.LongAspect = LongSideScore(particles.elementAt(0));
+					SmartDashboard.putNumber("Long Aspect", scores.LongAspect);
+					scores.ShortAspect = ShortSideScore(particles.elementAt(0));
+					SmartDashboard.putNumber("Short Aspect", scores.ShortAspect);
+					scores.AreaToConvexHullArea = ConvexHullAreaScore(particles.elementAt(0));
+					SmartDashboard.putNumber("Convex Hull Area", scores.AreaToConvexHullArea);
+					boolean isTote = scores.Trapezoid > SCORE_MIN && (scores.LongAspect > SCORE_MIN || scores.ShortAspect > SCORE_MIN) && scores.AreaToConvexHullArea > SCORE_MIN;
+					boolean isLong = scores.LongAspect > scores.ShortAspect;
+
+					//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
+					SmartDashboard.putBoolean("IsTote", isTote);
+					SmartDashboard.putNumber("Distance", computeDistance(binaryFrame, particles.elementAt(0), isLong));
+				} else {
+					SmartDashboard.putBoolean("IsTote", false);
+				}
+
+				Timer.delay(0.005);				// wait for a motor update time
+			}
+		}
+
+		public void operatorControl() {
+			while(isOperatorControl() && isEnabled()) {
+				Timer.delay(0.005);				// wait for a motor update time
+			}
+		}
+
+		//Comparator function for sorting particles. Returns true if particle 1 is larger
+		static boolean CompareParticleSizes(ParticleReport particle1, ParticleReport particle2)
+		{
+			//we want descending sort order
+			return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea;
+		}
+
+		/**
+		 * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
+		 * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
+		 */
+		double ratioToScore(double ratio)
+		{
+			return (Math.max(0, Math.min(100*(1-Math.abs(1-ratio)), 100)));
+		}
+
+		/**
+		 * Method to score convex hull area. This scores how "complete" the particle is. Particles with large holes will score worse than a filled in shape
+		 */
+		double ConvexHullAreaScore(ParticleReport report)
+		{
+			return ratioToScore((report.Area/report.ConvexHullArea)*1.18);
+		}
+
+		/**
+		 * Method to score if the particle appears to be a trapezoid. Compares the convex hull (filled in) area to the area of the bounding box.
+		 * The expectation is that the convex hull area is about 95.4% of the bounding box area for an ideal tote.
+		 */
+		double TrapezoidScore(ParticleReport report)
+		{
+			return ratioToScore(report.ConvexHullArea/((report.BoundingRectRight-report.BoundingRectLeft)*(report.BoundingRectBottom-report.BoundingRectTop)*.954));
+		}
+
+		/**
+		 * Method to score if the aspect ratio of the particle appears to match the long side of a tote.
+		 */
+		double LongSideScore(ParticleReport report)
+		{
+			return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))/LONG_RATIO);
+		}
+
+		/**
+		 * Method to score if the aspect ratio of the particle appears to match the short side of a tote.
+		 */
+		double ShortSideScore(ParticleReport report){
+			return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))/SHORT_RATIO);
+		}
+
+		/**
+		 * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics
+		 * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
+		 *
+		 * @param image The image to use for measuring the particle estimated rectangle
+		 * @param report The Particle Analysis Report for the particle
+		 * @param isLong Boolean indicating if the target is believed to be the long side of a tote
+		 * @return The estimated distance to the target in feet.
+		 */
+		double computeDistance (Image image, ParticleReport report, boolean isLong) {
+			double normalizedWidth, targetWidth;
+			NIVision.GetImageSizeResult size;
+
+			size = NIVision.imaqGetImageSize(image);
+			normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/size.width;
+			targetWidth = isLong ? 26.0 : 16.9;
+
+			return  targetWidth/(normalizedWidth*12*Math.tan(VIEW_ANGLE*Math.PI/(180*2)));
+		}
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Retro_src/Robot.java

@@ -0,0 +1,214 @@
+package $package;
+
+import java.lang.Math;
+import java.util.Comparator;
+import java.util.Vector;
+
+import com.ni.vision.NIVision;
+import com.ni.vision.NIVision.Image;
+import com.ni.vision.NIVision.ImageType;
+
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.SampleRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Example of finding yellow totes based on retroreflective target.
+ * This example utilizes an image file, which you need to copy to the roboRIO
+ * To use a camera you will have to integrate the appropriate camera details with this example.
+ * To use a USB camera instead, see the SimpelVision and AdvancedVision examples for details
+ * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on
+ * using an Axis Camera.
+ *
+ * Sample images can found here: http://wp.wpi.edu/wpilib/2015/01/16/sample-images-for-vision-projects/ 
+ */
+public class Robot extends SampleRobot {
+		//A structure to hold measurements of a particle
+		public class ParticleReport implements Comparator<ParticleReport>, Comparable<ParticleReport>{
+			double PercentAreaToImageArea;
+			double Area;
+			double BoundingRectLeft;
+			double BoundingRectTop;
+			double BoundingRectRight;
+			double BoundingRectBottom;
+			
+			public int compareTo(ParticleReport r)
+			{
+				return (int)(r.Area - this.Area);
+			}
+			
+			public int compare(ParticleReport r1, ParticleReport r2)
+			{
+				return (int)(r1.Area - r2.Area);
+			}
+		};
+
+		//Structure to represent the scores for the various tests used for target identification
+		public class Scores {
+			double Area;
+			double Aspect;
+		};
+
+		//Images
+		Image frame;
+		Image binaryFrame;
+		int imaqError;
+
+		//Constants
+		NIVision.Range TOTE_HUE_RANGE = new NIVision.Range(101, 64);	//Default hue range for yellow tote
+		NIVision.Range TOTE_SAT_RANGE = new NIVision.Range(88, 255);	//Default saturation range for yellow tote
+		NIVision.Range TOTE_VAL_RANGE = new NIVision.Range(134, 255);	//Default value range for yellow tote
+		double AREA_MINIMUM = 0.5; //Default Area minimum for particle as a percentage of total image area
+		double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22
+		double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4
+		double SCORE_MIN = 75.0;  //Minimum score to be considered a tote
+		double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480
+		NIVision.ParticleFilterCriteria2 criteria[] = new NIVision.ParticleFilterCriteria2[1];
+		NIVision.ParticleFilterOptions2 filterOptions = new NIVision.ParticleFilterOptions2(0,0,1,1);
+		Scores scores = new Scores();
+
+		public void robotInit() {
+		    // create images
+			frame = NIVision.imaqCreateImage(ImageType.IMAGE_RGB, 0);
+			binaryFrame = NIVision.imaqCreateImage(ImageType.IMAGE_U8, 0);
+			criteria[0] = new NIVision.ParticleFilterCriteria2(NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA, AREA_MINIMUM, 100.0, 0, 0);
+
+			//Put default values to SmartDashboard so fields will appear
+			SmartDashboard.putNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
+			SmartDashboard.putNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
+			SmartDashboard.putNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
+			SmartDashboard.putNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
+			SmartDashboard.putNumber("Tote val min", TOTE_VAL_RANGE.minValue);
+			SmartDashboard.putNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
+			SmartDashboard.putNumber("Area min %", AREA_MINIMUM);
+		}
+
+		public void autonomous() {
+			while (isAutonomous() && isEnabled())
+			{
+				//read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder to the
+				//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
+				NIVision.imaqReadFile(frame, "/home/lvuser/SampleImages/image.jpg");
+
+				//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
+				TOTE_HUE_RANGE.minValue = (int)SmartDashboard.getNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
+				TOTE_HUE_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
+				TOTE_SAT_RANGE.minValue = (int)SmartDashboard.getNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
+				TOTE_SAT_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
+				TOTE_VAL_RANGE.minValue = (int)SmartDashboard.getNumber("Tote val min", TOTE_VAL_RANGE.minValue);
+				TOTE_VAL_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
+
+				//Threshold the image looking for yellow (tote color)
+				NIVision.imaqColorThreshold(binaryFrame, frame, 255, NIVision.ColorMode.HSV, TOTE_HUE_RANGE, TOTE_SAT_RANGE, TOTE_VAL_RANGE);
+
+				//Send particle count to dashboard
+				int numParticles = NIVision.imaqCountParticles(binaryFrame, 1);
+				SmartDashboard.putNumber("Masked particles", numParticles);
+
+				//Send masked image to dashboard to assist in tweaking mask.
+				CameraServer.getInstance().setImage(binaryFrame);
+
+				//filter out small particles
+				float areaMin = (float)SmartDashboard.getNumber("Area min %", AREA_MINIMUM);
+				criteria[0].lower = areaMin;
+				imaqError = NIVision.imaqParticleFilter4(binaryFrame, binaryFrame, criteria, filterOptions, null);
+
+				//Send particle count after filtering to dashboard
+				numParticles = NIVision.imaqCountParticles(binaryFrame, 1);
+				SmartDashboard.putNumber("Filtered particles", numParticles);
+
+				if(numParticles > 0)
+				{
+					//Measure particles and sort by particle size
+					Vector<ParticleReport> particles = new Vector<ParticleReport>();
+					for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
+					{
+						ParticleReport par = new ParticleReport();
+						par.PercentAreaToImageArea = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA);
+						par.Area = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA);
+						par.BoundingRectTop = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_TOP);
+						par.BoundingRectLeft = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_LEFT);
+						par.BoundingRectBottom = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_BOTTOM);
+						par.BoundingRectRight = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_RIGHT);
+						particles.add(par);
+					}
+					particles.sort(null);
+
+					//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
+					//for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information 
+					//about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote.
+					scores.Aspect = AspectScore(particles.elementAt(0));
+					SmartDashboard.putNumber("Aspect", scores.Aspect);
+					scores.Area = AreaScore(particles.elementAt(0));
+					SmartDashboard.putNumber("Area", scores.Area);
+					boolean isTote = scores.Aspect > SCORE_MIN && scores.Area > SCORE_MIN;
+
+					//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
+					SmartDashboard.putBoolean("IsTote", isTote);
+					SmartDashboard.putNumber("Distance", computeDistance(binaryFrame, particles.elementAt(0)));
+				} else {
+					SmartDashboard.putBoolean("IsTote", false);
+				}
+
+				Timer.delay(0.005);				// wait for a motor update time
+			}
+		}
+
+		public void operatorControl() {
+			while(isOperatorControl() && isEnabled()) {
+				Timer.delay(0.005);				// wait for a motor update time
+			}
+		}
+
+		//Comparator function for sorting particles. Returns true if particle 1 is larger
+		static boolean CompareParticleSizes(ParticleReport particle1, ParticleReport particle2)
+		{
+			//we want descending sort order
+			return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea;
+		}
+
+		/**
+		 * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
+		 * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
+		 */
+		double ratioToScore(double ratio)
+		{
+			return (Math.max(0, Math.min(100*(1-Math.abs(1-ratio)), 100)));
+		}
+
+		double AreaScore(ParticleReport report)
+		{
+			double boundingArea = (report.BoundingRectBottom - report.BoundingRectTop) * (report.BoundingRectRight - report.BoundingRectLeft);
+			//Tape is 7" edge so 49" bounding rect. With 2" wide tape it covers 24" of the rect.
+			return ratioToScore((49/24)*report.Area/boundingArea);
+		}
+
+		/**
+		 * Method to score if the aspect ratio of the particle appears to match the retro-reflective target. Target is 7"x7" so aspect should be 1
+		 */
+		double AspectScore(ParticleReport report)
+		{
+			return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop)));
+		}
+
+		/**
+		 * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics
+		 * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
+		 *
+		 * @param image The image to use for measuring the particle estimated rectangle
+		 * @param report The Particle Analysis Report for the particle
+		 * @param isLong Boolean indicating if the target is believed to be the long side of a tote
+		 * @return The estimated distance to the target in feet.
+		 */
+		double computeDistance (Image image, ParticleReport report) {
+			double normalizedWidth, targetWidth;
+			NIVision.GetImageSizeResult size;
+
+			size = NIVision.imaqGetImageSize(image);
+			normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/size.width;
+			targetWidth = 7;
+
+			return  targetWidth/(normalizedWidth*12*Math.tan(VIEW_ANGLE*Math.PI/(180*2)));
+		}
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/MecanumDrive/src/org/usfirst/frc/team190/robot/Robot.java

@@ -25,10 +25,10 @@ public class Robot extends SampleRobot {
     final int joystickChannel	= 0;
 
     public Robot() {
-    	robotDrive.setExpiration(0.1);
         robotDrive = new RobotDrive(frontLeftChannel, rearLeftChannel, frontRightChannel, rearRightChannel);
     	robotDrive.setInvertedMotor(MotorType.kFrontLeft, true);	// invert the left side motors
     	robotDrive.setInvertedMotor(MotorType.kRearLeft, true);		// you may need to change or remove this to match your robot
+        robotDrive.setExpiration(0.1);
 
         stick = new Joystick(joystickChannel);
     }

eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml

@@ -325,6 +325,46 @@
         destination="src/$package-dir/Robot.java"></file>
     </files>
   </example>
+  
+  	<example>
+		<name>2015 Vision Color Sample</name>
+        <description>An example program that demonstrates image processing to locate Yellow totes by color.
+			This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing.
+			To use this code with a camera, you must integrate the code for image acquisition from the appropriate
+			camera example;
+        </description>
+        <tags>
+            <tag>Vision</tag>
+            <tag>Complete List</tag>
+        </tags>
+        <packages>
+            <package>src/$package-dir</package>
+        </packages>
+        <files>
+            <file source="examples/2015Vision/Color_src/Robot.java"
+                  destination="src/$package-dir/Robot.java"></file>
+		</files>
+    </example>
+
+	<example>
+		<name>2015 Vision Retro Sample</name>
+        <description>An example program that demonstrates image processing to locate Yellow totes by the retroreflective target.
+			This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing.
+			To use this code with a camera, you must integrate the code for image acquisition from the appropriate
+			camera example;
+        </description>
+        <tags>
+            <tag>Vision</tag>
+            <tag>Complete List</tag>
+        </tags>
+        <packages>
+            <package>src/$package-dir</package>
+        </packages>
+        <files>
+            <file source="examples/2015Vision/Retro_src/Robot.java"
+                  destination="src/$package-dir/Robot.java"></file>
+		</files>
+    </example>
 
     <example>
         <name>Axis Camera Sample</name>

eclipse-plugins/edu.wpi.first.wpilib.plugins.java/src/main/resources/java-zip/ant/build.xml

@@ -104,7 +104,16 @@
   <target name="deploy" depends="clean,jar,get-target-ip,dependencies" description="Deploy the jar and start the program running.">
     <echo>[athena-deploy] Copying code over.</echo>
     <scp file="${dist.jar}" todir="${username}@${target}:${deploy.dir}" password="${password}" trust="true"/>
-    <scp file="${wpilib.ant.dir}/robotCommand" todir="${username}@${target}:${command.dir}" password="${password}" trust="true"/>
+
+      <sshexec host="${target}"
+               username="admin"
+               password="${password}"
+               trust="true"
+               failonerror="false"
+               command="killall netconsole-host"/>
+      <scp file="${wpilib.ant.dir}/netconsole-host" todir="admin@${target}:/usr/local/frc/bin" password="${password}" trust="true"/>
+
+      <scp file="${wpilib.ant.dir}/robotCommand" todir="${username}@${target}:${command.dir}" password="${password}" trust="true"/>
 
     <echo>[athena-deploy] Starting program.</echo>
     <sshexec host="${target}"

hal/include/HAL/Digital.hpp

@@ -86,6 +86,8 @@ extern "C"
 	void setEncoderSamplesToAverage(void* encoder_pointer, uint32_t samplesToAverage,
 			int32_t *status);
 	uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status);
+	void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh,
+			bool edgeSensitive, int32_t *status);
 
 	uint16_t getLoopTiming(int32_t *status);
 

hal/lib/Athena/Digital.cpp

@@ -632,6 +632,25 @@ void setCounterAverageSize(void* counter_pointer, int32_t size, int32_t *status)
   counter->counter->writeTimerConfig_AverageSize(size, status);
 }
 
+/**
+ * remap the digital source pin and set the module.
+ * If it's an analog trigger, determine the module from the high order routing channel
+ * else do normal digital input remapping based on pin number (MXP)
+ */
+void remapDigitalSource(bool analogTrigger, uint32_t &pin, uint8_t &module) {
+  if (analogTrigger) {
+    module = pin >> 4;
+  } else {
+    if(pin >= kNumHeaders) {
+      pin = remapMXPChannel(pin);
+      module = 1;
+    } else {
+      module = 0;
+    }
+  }
+}
+
+
 /**
  * Set the source object that causes the counter to count up.
  * Set the up counting DigitalSource.
@@ -641,12 +660,7 @@ void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger,
 
   uint8_t module;
 
-  if(pin >= kNumHeaders) {
-    pin = remapMXPChannel(pin);
-    module = 1;
-  } else {
-    module = 0;
-  }
+  remapDigitalSource(analogTrigger, pin, module);
 
   counter->counter->writeConfig_UpSource_Module(module, status);
   counter->counter->writeConfig_UpSource_Channel(pin, status);
@@ -696,12 +710,7 @@ void setCounterDownSource(void* counter_pointer, uint32_t pin, bool analogTrigge
 
   uint8_t module;
 
-  if(pin >= kNumHeaders) {
-    pin = remapMXPChannel(pin);
-    module = 1;
-  } else {
-    module = 0;
-  }
+  remapDigitalSource(analogTrigger, pin, module);
 
   counter->counter->writeConfig_DownSource_Module(module, status);
   counter->counter->writeConfig_DownSource_Channel(pin, status);
@@ -925,15 +934,8 @@ void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_
   // Initialize encoder structure
   Encoder* encoder = new Encoder();
 
-  if(port_a_pin >= kNumHeaders) {
-    port_a_pin = remapMXPChannel(port_a_pin);
-    port_a_module = 1;
-  }
-
-  if(port_b_pin >= kNumHeaders) {
-    port_b_pin = remapMXPChannel(port_b_pin);
-    port_b_module = 1;
-  }
+  remapDigitalSource(port_a_analog_trigger, port_a_pin, port_a_module);
+  remapDigitalSource(port_b_analog_trigger, port_b_pin, port_b_module);
 
   Resource::CreateResourceObject(&quadEncoders, tEncoder::kNumSystems);
   encoder->index = quadEncoders->Allocate("4X Encoder");
@@ -1078,6 +1080,20 @@ uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status) {
   return encoder->encoder->readTimerConfig_AverageSize(status);
 }
 
+/**
+ * Set an index source for an encoder, which is an input that resets the
+ * encoder's count.
+ */
+void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh,
+    bool edgeSensitive, int32_t *status) {
+  Encoder* encoder = (Encoder*) encoder_pointer;
+  encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status);
+  encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status);
+  encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger, status);
+  encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status);
+  encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status);
+}
+
 /**
  * Get the loop timing of the PWM system
  *

hal/lib/Athena/Interrupts.cpp

@@ -1,6 +1,8 @@
 #include "HAL/Interrupts.hpp"
 #include "ChipObject.h"
 
+extern void remapDigitalSource(bool analogTrigger, uint32_t &pin, uint8_t &module);
+
 struct Interrupt // FIXME: why is this internal?
 {
 	tInterrupt *anInterrupt;
@@ -99,6 +101,7 @@ void requestInterrupts(void* interrupt_pointer, uint8_t routing_module, uint32_t
 {
 	Interrupt* anInterrupt = (Interrupt*)interrupt_pointer;
 	anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
+	remapDigitalSource(routing_analog_trigger, routing_pin, routing_module);
 	anInterrupt->anInterrupt->writeConfig_Source_AnalogTrigger(routing_analog_trigger, status);
 	anInterrupt->anInterrupt->writeConfig_Source_Channel(routing_pin, status);
 	anInterrupt->anInterrupt->writeConfig_Source_Module(routing_module, status);

hal/lib/Athena/ctre/PCM.cpp

@@ -13,9 +13,9 @@ static const INT32 kCANPeriod = 20;
 #define STATUS_DEBUG  		0x9041480
 
 #define EXPECTED_RESPONSE_TIMEOUT_MS	(50)
-#define GET_PCM_STATUS()			CtreCanNode::recMsg<PcmStatus_t> 		rx = GetRx<PcmStatus_t>			(STATUS_1,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_PCM_SOL_FAULTS()		CtreCanNode::recMsg<PcmStatusFault_t> 	rx = GetRx<PcmStatusFault_t>	(STATUS_SOL_FAULTS,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_PCM_DEBUG()				CtreCanNode::recMsg<PcmDebug_t> 		rx = GetRx<PcmDebug_t>			(STATUS_DEBUG,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_STATUS()			CtreCanNode::recMsg<PcmStatus_t> 		rx = GetRx<PcmStatus_t>			(STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_SOL_FAULTS()		CtreCanNode::recMsg<PcmStatusFault_t> 	rx = GetRx<PcmStatusFault_t>	(STATUS_SOL_FAULTS|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_DEBUG()				CtreCanNode::recMsg<PcmDebug_t> 		rx = GetRx<PcmDebug_t>			(STATUS_DEBUG|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
 
 #define CONTROL_1 			0x09041C00	/* PCM_Control */
 #define CONTROL_2 			0x09041C40	/* PCM_SupplemControl */

hal/lib/Athena/ctre/PDP.cpp

@@ -178,7 +178,8 @@ CTR_Code PDP::GetTotalEnergy(double &energyJoules)
 	raw <<= 8;
 	raw |=  rx->Energy_125mWPerUnitXTmeas_l8;
 	energyJoules = 0.125 * raw; 						/* mW integrated every TmeasMs */
-	energyJoules *= rx->TmeasMs_likelywillbe20ms_;	/* multiplied by TmeasMs = joules */
+	energyJoules *= 0.001;								/* convert from mW to W */
+	energyJoules *= rx->TmeasMs_likelywillbe20ms_;		/* multiplied by TmeasMs = joules */
 	return rx.err;
 }
 /* Clear sticky faults.

jenkins/docs/cpp.doxy

@@ -234,7 +234,7 @@ TCL_SUBST              =
 # members will be omitted, etc.
 # The default value is: NO.
 
-OPTIMIZE_OUTPUT_FOR_C  = YES
+OPTIMIZE_OUTPUT_FOR_C  = NO
 
 # Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
 # Python sources only. Doxygen will then generate output that is more tailored

networktables/cpp/include/networktables2/AbstractNetworkTableEntryStore.h

@@ -35,6 +35,8 @@ public:
 
 class AbstractNetworkTableEntryStore : public IncomingEntryReceiver{
 protected:
+
+	NTReentrantSemaphore block_namedEntries;
 	std::map<EntryId,NetworkTableEntry*> idEntries;
 	std::map<std::string,NetworkTableEntry*> namedEntries;
 	TableListenerManager& listenerManager;
@@ -45,10 +47,10 @@ protected:
 	OutgoingEntryReceiver* incomingReceiver;
 
 
-	
+
 	virtual bool addEntry(NetworkTableEntry* entry) = 0;
 	virtual bool updateEntry(NetworkTableEntry* entry, SequenceNumber sequenceNumber, EntryValue value) = 0;
-	
+
 public:
 	virtual ~AbstractNetworkTableEntryStore();
 
@@ -62,14 +64,14 @@ public:
 
 	void SetOutgoingReceiver(OutgoingEntryReceiver* receiver);
 	void SetIncomingReceiver(OutgoingEntryReceiver* receiver);
-	
+
 	void PutOutgoing(std::string& name, NetworkTableEntryType* type, EntryValue value);
 	void PutOutgoing(NetworkTableEntry* tableEntry, EntryValue value);
-	
+
 
 	void offerIncomingAssignment(NetworkTableEntry* entry);
 	void offerIncomingUpdate(NetworkTableEntry* entry, EntryId sequenceNumber, EntryValue value);
-	
+
 	void notifyEntries(ITable* table, ITableListener* listener);
 
 };

networktables/cpp/include/networktables2/NetworkTableEntry.h

@@ -42,6 +42,7 @@ public:
 	
 	NetworkTableEntry(std::string& name, NetworkTableEntryType* type, EntryValue value);
 	NetworkTableEntry(EntryId id, std::string& name, SequenceNumber sequenceNumber, NetworkTableEntryType* type, EntryValue value);
+	NetworkTableEntry(const NetworkTableEntry &);
 	virtual ~NetworkTableEntry();
 
 	EntryId GetId();

networktables/cpp/lib/Athena/networktables2/stream/FDIOStream.cpp

@@ -15,6 +15,7 @@
 #include <iolib.h>
 #else
 #include <unistd.h>
+#include <fcntl.h>
 #endif
 #include <stdio.h>
 
@@ -24,6 +25,13 @@ FDIOStream::FDIOStream(int _fd){
   //	f = fdopen(_fd, "rbwb");
   //	if(f==NULL)
   //		throw IOException("Could not open stream from file descriptor", errno);
+  // Set the TCP socket to be non-blocking
+  int flags = fcntl(fd, F_GETFL, 0);
+  if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0)
+  {
+    ::close(fd);
+    throw IOException("Could not set socket to non-blocking mode");
+  }
 }
 FDIOStream::~FDIOStream(){
 	close();
@@ -66,14 +74,35 @@ int FDIOStream::read(void* ptr, int numbytes){
 	return totalRead;
 }
 int FDIOStream::write(const void* ptr, int numbytes){
-  int numWrote = ::write(fd, (char*)ptr, numbytes);//TODO: this is bad
-  //int numWrote = fwrite(ptr, 1, numbytes, f);
-  if(numWrote==numbytes)
-    return numWrote;
-  perror("write error: ");
-  fflush(stderr);
-  throw IOException("Could not write all bytes to fd stream");
-	
+	int numWrote = ::write(fd, (char*)ptr, numbytes);
+	if(numWrote==numbytes)
+		return numWrote;
+
+	if (numWrote == -1 && (errno == EWOULDBLOCK || errno == EAGAIN))
+	{
+		// see if write timeout expires
+		struct timeval timeout;
+		fd_set fdSet;
+
+		FD_ZERO(&fdSet);
+		FD_SET(fd, &fdSet);
+		timeout.tv_sec = 1;		// wait 1 second for the other side to connect
+		timeout.tv_usec = 0;
+
+		int select_result = select(FD_SETSIZE, NULL, &fdSet, NULL, &timeout);
+		if ( select_result < 0)
+			throw IOException("Select returned an error on write");
+
+		if (FD_ISSET(fd, &fdSet)) {
+			numWrote = ::write(fd, (char*)ptr, numbytes);
+			if(numWrote==numbytes)
+				return numWrote;
+		}
+	}
+ 	
+	perror("write error: ");
+	fflush(stderr);
+	throw IOException("Could not write all bytes to fd stream");
 }
 void FDIOStream::flush(){
   //if(fflush(f)==EOF)

networktables/cpp/lib/Win32/src/main/native/OSAL/Task.cpp

@@ -1,6 +1,8 @@
 #include "stdafx.h"
 #include "OSAL/Task.h"
 
+#include "NetworkCommunication/UsageReporting.h"
+#include "WPIErrors.h"
 #include <string.h>
 #include <Windows.h>
 
@@ -55,7 +57,7 @@ DWORD thread_proc( void *p_ptr )
 //This sets the name of the thread, which can help to identify threads in win32
 #define MS_VC_EXCEPTION 0x406D1388
 static void set_thread_name( const char *p_thread_name, DWORD ID )
-{	
+{
 #pragma pack(push,8)
 	typedef struct tagTHREADNAME_INFO
 	{	DWORD dwType;		// Must be 0x1000.
@@ -162,7 +164,7 @@ bool NTTask::Stop()
 		::TerminateThread( m_Handle, 0 );
 
 		// Free the memory
-		::VirtualFree( Info_.AllocationBase, 0, MEM_RELEASE ); 		
+		::VirtualFree( Info_.AllocationBase, 0, MEM_RELEASE );
 	}
 
 	//if (Verify())
@@ -289,19 +291,19 @@ bool NTTask::HandleError(STATUS results)
 	//case S_objLib_OBJ_ID_ERROR:
 	//	wpi_setWPIErrorWithContext(TaskIDError, m_taskName);
 	//	break;
-	//	
+	//
 	//case S_objLib_OBJ_DELETED:
 	//	wpi_setWPIErrorWithContext(TaskDeletedError, m_taskName);
 	//	break;
-	//	
+	//
 	//case S_taskLib_ILLEGAL_OPTIONS:
 	//	wpi_setWPIErrorWithContext(TaskOptionsError, m_taskName);
 	//	break;
-	//	
+	//
 	//case S_memLib_NOT_ENOUGH_MEMORY:
 	//	wpi_setWPIErrorWithContext(TaskMemoryError, m_taskName);
 	//	break;
-	//	
+	//
 	//case S_taskLib_ILLEGAL_PRIORITY:
 	//	wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName);
 	//	break;
@@ -312,4 +314,3 @@ bool NTTask::HandleError(STATUS results)
 	//}
 	return false;
 }
-

networktables/cpp/lib/Win32/src/main/native/networktables2/stream/FDIOStream.cpp

@@ -20,6 +20,13 @@
 
 FDIOStream::FDIOStream(int _fd){
   fd = _fd;
+  // Set the TCP socket to be non-blocking
+  u_long on = 1;
+  if (ioctlsocket(fd, FIONBIO, &on))
+  {
+    ::close(fd);
+    throw IOException("Could not set socket to non-blocking mode");
+  }
 }
 
 FDIOStream::~FDIOStream(){
@@ -79,13 +86,35 @@ int Send( int sockfd,char* Data, size_t sizeData )
 
 int FDIOStream::write(const void* ptr, int numbytes)
 {
-  int numWrote = Send(fd,(char *)ptr,numbytes);
-  if(numWrote==numbytes)
-    return numWrote;
-  perror("write error: ");
-  fflush(stderr);
-  throw IOException("Could not write all bytes to fd stream");
-	
+	int numWrote = ::write(fd, (char*)ptr, numbytes);
+	if(numWrote==numbytes)
+		return numWrote;
+
+	if (numWrote == -1 && (errno == EWOULDBLOCK || errno == EAGAIN))
+	{
+		// see if write timeout expires
+		struct timeval timeout;
+		fd_set fdSet;
+
+		FD_ZERO(&fdSet);
+		FD_SET(fd, &fdSet);
+		timeout.tv_sec = 1;		// wait 1 second for the other side to connect
+		timeout.tv_usec = 0;
+
+		int select_result = select(FD_SETSIZE, NULL, &fdSet, NULL, &timeout);
+		if ( select_result < 0)
+			throw IOException("Select returned an error on write");
+
+		if (FD_ISSET(fd, &fdSet)) {
+			numWrote = ::write(fd, (char*)ptr, numbytes);
+			if(numWrote==numbytes)
+				return numWrote;
+		}
+	}
+ 	
+	perror("write error: ");
+	fflush(stderr);
+	throw IOException("Could not write all bytes to fd stream");
 }
 
 void FDIOStream::flush(){

networktables/cpp/lib/share/networktables2/AbstractNetworkTableEntryStore.cpp

@@ -12,10 +12,10 @@
 #include <iostream>
 #include <stdio.h>
 
-	AbstractNetworkTableEntryStore::AbstractNetworkTableEntryStore(TableListenerManager& lstnManager): 
+	AbstractNetworkTableEntryStore::AbstractNetworkTableEntryStore(TableListenerManager& lstnManager):
 			listenerManager(lstnManager){
 	}
-	
+
 	AbstractNetworkTableEntryStore::~AbstractNetworkTableEntryStore(){
 		LOCK.take();
 		std::map<std::string, NetworkTableEntry*>::iterator itr;
@@ -24,15 +24,15 @@
 		    namedEntries.erase(itr++);
 		}
 	}
-	
+
 	/**
 	 * Get an entry based on it's name
 	 * @param name the name of the entry to look for
 	 * @return the entry or null if the entry does not exist
 	 */
 	NetworkTableEntry* AbstractNetworkTableEntryStore::GetEntry(std::string& name){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			std::map<std::string, NetworkTableEntry*>::iterator value_itr = namedEntries.find(name);
 			if(value_itr != namedEntries.end()) {
 				return value_itr->second;
@@ -40,11 +40,11 @@
 			return NULL;
 		}
 	}
-	
+
 	NetworkTableEntry* AbstractNetworkTableEntryStore::GetEntry(EntryId entryId){
-		{ 
-			NTSynchronized sync(LOCK);
-			
+		{
+			NTSynchronized sync(block_namedEntries);
+
 			std::map<EntryId, NetworkTableEntry*>::iterator value_itr = idEntries.find(entryId);
 			if(value_itr != idEntries.end()) {
 				return value_itr->second;
@@ -52,74 +52,74 @@
 			return NULL;
 		}
 	}
-	
+
 	std::vector<std::string>* AbstractNetworkTableEntryStore::keys(){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			std::vector<std::string>* keys = new std::vector<std::string>();
 			std::map<std::string, NetworkTableEntry*>::iterator itr;
-			
+
 			for(itr = namedEntries.begin(); itr != namedEntries.end(); itr++)
 			{
 				std::string key = (*itr).first;
 				keys->push_back(key);
 			}
-			
+
 			return (keys);
 		}
-		
+
 	}
-	
+
 	/**
 	 * Remove all entries
          * NOTE: This method should not be used with applications which cache entries which would lead to unknown results
          * This method is for use in testing only
 	 */
 	void AbstractNetworkTableEntryStore::clearEntries(){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			namedEntries.clear();
 			idEntries.clear();
 		}
 	}
-	
+
 	/**
 	 * clear the id's of all entries
 	 */
 	void AbstractNetworkTableEntryStore::clearIds(){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			std::map<std::string, NetworkTableEntry*>::iterator itr;
 			idEntries.clear();
-			
+
 			for(itr = namedEntries.begin(); itr != namedEntries.end(); itr++)
 			{
 				((NetworkTableEntry*)(*itr).second)->ClearId();
 			}
 		}
 	}
-	
+
 	void AbstractNetworkTableEntryStore::SetOutgoingReceiver(OutgoingEntryReceiver* receiver){
 		outgoingReceiver = receiver;
 	}
-	
+
 	void AbstractNetworkTableEntryStore::SetIncomingReceiver(OutgoingEntryReceiver* receiver){
 		incomingReceiver = receiver;
 	}
-	
+
 	/**
-	 * Stores the given value under the given name and queues it for 
+	 * Stores the given value under the given name and queues it for
 	 * transmission to the server.
-	 * 
+	 *
 	 * @param name The name under which to store the given value.
 	 * @param type The type of the given value.
 	 * @param value The value to store.
-	 * @throws TableKeyExistsWithDifferentTypeException Thrown if an 
+	 * @throws TableKeyExistsWithDifferentTypeException Thrown if an
 	 *  entry already exists with the given name and is of a different type.
 	 */
 	void AbstractNetworkTableEntryStore::PutOutgoing(std::string& name, NetworkTableEntryType* type, EntryValue value){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			std::map<std::string, NetworkTableEntry*>::iterator index = namedEntries.find(name);
 			NetworkTableEntry* tableEntry;
 			if(index == namedEntries.end())//if the name does not exist in the current entries
@@ -137,21 +137,21 @@
 				if(tableEntry->GetType()->id != type->id){
 					throw TableKeyExistsWithDifferentTypeException(name, tableEntry->GetType());
 				}
-				
+
 				EntryValue oldValue = tableEntry->GetValue();
 				if(!type->areEqual(value, oldValue)){
 				  if(updateEntry(tableEntry, (SequenceNumber)(tableEntry->GetSequenceNumber() + 1), value)){
 				    outgoingReceiver->offerOutgoingUpdate(tableEntry);
 				  }
-				
+
 				  tableEntry->FireListener(listenerManager);
 				}
 			}
 		}
 	}
-	
+
 	void AbstractNetworkTableEntryStore::PutOutgoing(NetworkTableEntry* tableEntry, EntryValue value){
-		{ 
+		{
 			NTSynchronized sync(LOCK);
 			NetworkTableEntryType* type = tableEntry->GetType();
 			EntryValue oldValue = tableEntry->GetValue();
@@ -159,15 +159,15 @@
 			  if(updateEntry(tableEntry, (SequenceNumber)(tableEntry->GetSequenceNumber() + 1), value)){
 			    outgoingReceiver->offerOutgoingUpdate(tableEntry);
 			  }
-			
+
 			  tableEntry->FireListener(listenerManager);
 			}
 		}
 	}
-	
+
 	void AbstractNetworkTableEntryStore::offerIncomingAssignment(NetworkTableEntry* entry){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			std::map<std::string, NetworkTableEntry*>::iterator itr = namedEntries.find(entry->name);
 			NetworkTableEntry* tableEntry;
 			if(addEntry(entry)){
@@ -177,7 +177,7 @@
 				else{
 					tableEntry = entry;
 				}
-				
+
 				tableEntry->FireListener(listenerManager);//if we didnt have a pointer, then the copy of the version in the list would call this method, however with the pointer we are updating the version in the list
 				incomingReceiver->offerOutgoingAssignment(tableEntry);
 			}
@@ -185,9 +185,9 @@
 			  delete entry;
 		}
 	}
-	
+
 	void AbstractNetworkTableEntryStore::offerIncomingUpdate(NetworkTableEntry* entry, SequenceNumber squenceNumber, EntryValue value){
-		{ 
+		{
 			NTSynchronized sync(LOCK);
 			if(updateEntry(entry, squenceNumber, value)){
 				entry->FireListener(listenerManager);
@@ -195,15 +195,15 @@
 			}
 		}
 	}
-	
+
 	/**
 	 * Called to say that a listener should notify the listener manager of all of the entries
 	 * @param listener
-	 * @param table 
+	 * @param table
 	 */
 	void AbstractNetworkTableEntryStore::notifyEntries(ITable* table, ITableListener* listener){
-		{ 
-			NTSynchronized sync(LOCK);
+		{
+			NTSynchronized sync(block_namedEntries);
 			std::map<std::string, NetworkTableEntry*>::iterator itr;
 			for(itr = namedEntries.begin(); itr != namedEntries.end(); itr++)
 			{

networktables/cpp/lib/share/networktables2/NetworkTableEntry.cpp

@@ -20,6 +20,17 @@ NetworkTableEntry::NetworkTableEntry(EntryId _id, std::string& _name, SequenceNu
 	m_isDirty = false;
 }
 
+NetworkTableEntry::NetworkTableEntry(const NetworkTableEntry &other) :
+	name(other.name),
+	id(other.id),
+	sequenceNumber(other.sequenceNumber),
+	type(other.type),
+	m_isNew(other.m_isNew),
+	m_isDirty(other.m_isDirty)
+{
+	value = type->copyValue(other.value);
+}
+
 NetworkTableEntry::~NetworkTableEntry(){
   type->deleteValue(value);
 }

networktables/cpp/lib/share/networktables2/WriteManager.cpp

@@ -54,10 +54,15 @@ void WriteManager::stop(){
 
 void WriteManager::offerOutgoingAssignment(NetworkTableEntry* entry) {
 	{ 
-		NTSynchronized sync(transactionsLock);
-		((std::queue<NetworkTableEntry*>*)incomingAssignmentQueue)->push(entry);
-		
-		if(((std::queue<NetworkTableEntry*>*)incomingAssignmentQueue)->size()>=queueSize){
+		bool test_queue_overflow=false;
+		{
+			NTSynchronized sync(transactionsLock);
+			((std::queue<NetworkTableEntry*>*)incomingAssignmentQueue)->push(entry);
+
+			test_queue_overflow=(((std::queue<NetworkTableEntry*>*)incomingAssignmentQueue)->size()>=queueSize);
+		}
+		if (test_queue_overflow)
+		{
 			run();
 			writeWarning("assignment queue overflowed. decrease the rate at which you create new entries or increase the write buffer size");
 		}
@@ -67,9 +72,15 @@ void WriteManager::offerOutgoingAssignment(NetworkTableEntry* entry) {
 
 void WriteManager::offerOutgoingUpdate(NetworkTableEntry* entry) {
 	{ 
-		NTSynchronized sync(transactionsLock);
-		((std::queue<NetworkTableEntry*>*)incomingUpdateQueue)->push(entry);
-		if(((std::queue<NetworkTableEntry*>*)incomingUpdateQueue)->size()>=queueSize){
+		bool test_queue_overflow=false;
+		{
+			NTSynchronized sync(transactionsLock);
+			((std::queue<NetworkTableEntry*>*)incomingUpdateQueue)->push(entry);
+			test_queue_overflow=(((std::queue<NetworkTableEntry*>*)incomingUpdateQueue)->size()>=queueSize);
+
+		}
+		if (test_queue_overflow)
+		{
 			run();
 			writeWarning("update queue overflowed. decrease the rate at which you update entries or increase the write buffer size");
 		}
@@ -97,10 +108,17 @@ void WriteManager::run() {
 		entry = ((std::queue<NetworkTableEntry*>*)outgoingAssignmentQueue)->front();
 		((std::queue<NetworkTableEntry*>*)outgoingAssignmentQueue)->pop();
 		{
-			NTSynchronized sync(entryStore.LOCK);
-			entry->MakeClean();
-			wrote = true;
-			receiver.offerOutgoingAssignment(entry);
+			NetworkTableEntry * entryCopy;
+
+			{
+				NTSynchronized sync(entryStore.LOCK);
+				entry->MakeClean();
+				wrote = true;
+				entryCopy = new NetworkTableEntry(*entry);
+			}
+
+			receiver.offerOutgoingAssignment(entryCopy);
+			delete entryCopy;
 		}
 	}
 	
@@ -108,10 +126,17 @@ void WriteManager::run() {
 		entry = ((std::queue<NetworkTableEntry*>*)outgoingUpdateQueue)->front();
 		((std::queue<NetworkTableEntry*>*)outgoingUpdateQueue)->pop();
 		{ 
-			NTSynchronized sync(entryStore.LOCK);
-			entry->MakeClean();
-			wrote = true;
-			receiver.offerOutgoingUpdate(entry);
+			NetworkTableEntry * entryCopy;
+
+			{
+				NTSynchronized sync(entryStore.LOCK);
+				entry->MakeClean();
+				wrote = true;
+				entryCopy = new NetworkTableEntry(*entry);
+			}
+
+			receiver.offerOutgoingUpdate(entryCopy);
+			delete entryCopy;
 		}
 	}
 	

networktables/cpp/lib/share/networktables2/client/ClientNetworkTableEntryStore.cpp

@@ -17,7 +17,7 @@ ClientNetworkTableEntryStore::~ClientNetworkTableEntryStore(){}
 
 bool ClientNetworkTableEntryStore::addEntry(NetworkTableEntry* newEntry){
 	{
-		NTSynchronized sync(LOCK);
+		NTSynchronized sync(block_namedEntries);
 		NetworkTableEntry* entry = (NetworkTableEntry*)namedEntries[newEntry->name];
 
 		if(entry!=NULL){
@@ -28,7 +28,7 @@ bool ClientNetworkTableEntryStore::addEntry(NetworkTableEntry* newEntry){
 					idEntries[newEntry->GetId()] = entry;
 				}
 			}
-			
+
 			entry->ForcePut(newEntry->GetSequenceNumber(), newEntry->GetType(), newEntry->GetValue());
 		}
 		else{
@@ -41,7 +41,7 @@ bool ClientNetworkTableEntryStore::addEntry(NetworkTableEntry* newEntry){
 }
 
 bool ClientNetworkTableEntryStore::updateEntry(NetworkTableEntry* entry, SequenceNumber sequenceNumber, EntryValue value) {
-	{ 
+	{
 		NTSynchronized sync(LOCK);
 		entry->ForcePut(sequenceNumber, value);
 		if(entry->GetId()==NetworkTableEntry::UNKNOWN_ID){
@@ -57,7 +57,7 @@ bool ClientNetworkTableEntryStore::updateEntry(NetworkTableEntry* entry, Sequenc
  * @throws IOException
  */
 void ClientNetworkTableEntryStore::sendUnknownEntries(NetworkTableConnection& connection) {
-	{ 
+	{
 		NTSynchronized sync(LOCK);
 		std::map<std::string, NetworkTableEntry*>::iterator itr;
 		for(itr = namedEntries.begin(); itr != namedEntries.end(); itr++)

networktables/cpp/lib/share/networktables2/connection/NetworkTableConnection.cpp

@@ -1,6 +1,6 @@
 /**
  * An abstraction for the NetworkTable protocol
- * 
+ *
  * @author mwills
  *
  */
@@ -19,6 +19,7 @@ NetworkTableConnection::~NetworkTableConnection(){
 
 void NetworkTableConnection::SetIOStream(IOStream* stream)
 {
+	isValid=(stream!=NULL);
 	ioStream->SetIOStream(stream);  //just passing through
 }
 
@@ -102,7 +103,7 @@ void NetworkTableConnection::sendEntryUpdate(NetworkTableEntry& entry) {
 }
 
 void NetworkTableConnection::read(ConnectionAdapter& adapter) {
-	int messageType = ioStream->readByte();
+	int messageType = (isValid)?ioStream->readByte():(int)KEEP_ALIVE;
 	switch (messageType) {
 	case KEEP_ALIVE:
 		adapter.keepAlive();

networktables/cpp/lib/share/networktables2/server/NetworkTableServer.cpp

@@ -17,10 +17,10 @@ NetworkTableServer::NetworkTableServer(IOStreamProvider& _streamProvider, Networ
 		connectionList(&incomingStreamMonitor),
 		writeManager(connectionList, threadManager, GetEntryStore(), ULONG_MAX),
                 continuingReceiver(writeManager){
-	
+
 	GetEntryStore().SetIncomingReceiver(&continuingReceiver);
 	GetEntryStore().SetOutgoingReceiver(&continuingReceiver);
-	
+
 	incomingStreamMonitor.start();
 	writeManager.start();
 }
@@ -35,6 +35,9 @@ NetworkTableServer::~NetworkTableServer(){
 
 void NetworkTableServer::Close(){
 	try{
+		//Note: streamProvider must come before the incomingStreamMonitor so the that task can complete first for the thread to close
+		//  [9/1/2013 Terminator]
+		streamProvider.close();
 		incomingStreamMonitor.stop();
 		writeManager.stop();
 		connectionList.closeAll();

networktables/cpp/lib/share/networktables2/server/ServerConnectionAdapter.cpp

@@ -36,6 +36,9 @@ void ServerConnectionAdapter::badMessage(BadMessageException& e) {
 	gotoState(new ServerConnectionState_Error(e));
 	adapterListener.close(*this, true);
 	m_IsAdapterListenerClosed=true;
+	if (readThread) {
+ 		readThread->stop();
+ 	}
 }
 
 void ServerConnectionAdapter::ioException(IOException& e) {
@@ -46,6 +49,9 @@ void ServerConnectionAdapter::ioException(IOException& e) {
 	else
 		gotoState(new ServerConnectionState_Error(e));
 	adapterListener.close(*this, false);
+	if (readThread) {
+		readThread->stop();
+	}
 	m_IsAdapterListenerClosed=true;
 }
 

networktables/cpp/lib/share/networktables2/server/ServerIncomingStreamMonitor.cpp

@@ -7,6 +7,7 @@
 
 #include "networktables2/server/ServerIncomingStreamMonitor.h"
 #include "networktables2/stream/IOStream.h"
+#include "networktables2/util/System.h"
 
 ServerIncomingStreamMonitor::ServerIncomingStreamMonitor(IOStreamProvider& _streamProvider, ServerNetworkTableEntryStore& _entryStore,
 		ServerIncomingConnectionListener& _incomingListener, ServerAdapterManager& _adapterListener, NetworkTableEntryTypeManager& _typeManager,
@@ -69,6 +70,7 @@ void ServerIncomingStreamMonitor::run()
 				ServerConnectionAdapter* connectionAdapter = new ServerConnectionAdapter(newStream, entryStore, entryStore, adapterListener, typeManager, threadManager);
 				incomingListener.OnNewConnection(*connectionAdapter);
 			}
+			sleep_ms(100); //avoid busy wait
 		}
 	}
 	catch (IOException& e)

networktables/cpp/lib/share/networktables2/server/ServerNetworkTableEntryStore.cpp

@@ -18,9 +18,9 @@ ServerNetworkTableEntryStore::~ServerNetworkTableEntryStore()
 
 bool ServerNetworkTableEntryStore::addEntry(NetworkTableEntry* newEntry)
 {
-	NTSynchronized sync(LOCK);
+	NTSynchronized sync(block_namedEntries);
 	NetworkTableEntry* entry = namedEntries[newEntry->name];
-	
+
 	if (entry == NULL)
 	{
 		newEntry->SetId(nextId++);
@@ -44,13 +44,19 @@ bool ServerNetworkTableEntryStore::updateEntry(NetworkTableEntry* entry, Sequenc
  */
 void ServerNetworkTableEntryStore::sendServerHello(NetworkTableConnection& connection)
 {
-	NTSynchronized sync(LOCK);
-	std::map<std::string, NetworkTableEntry*>::iterator itr;
-	for (itr = namedEntries.begin(); itr != namedEntries.end(); itr++)
+	std::vector<NetworkTableEntry *> entry_list;
 	{
-		NetworkTableEntry* entry = itr->second;
-		connection.sendEntryAssignment(*entry);
+		NTSynchronized sync(block_namedEntries);
+		std::map<std::string, NetworkTableEntry*>::iterator itr;
+		for (itr = namedEntries.begin(); itr != namedEntries.end(); itr++)
+		{
+			NetworkTableEntry* entry = itr->second;
+			entry_list.push_back(entry);
+		}
 	}
+
+	for (size_t i=0;i<entry_list.size();i++)
+					connection.sendEntryAssignment(*(entry_list[i]));
 	connection.sendServerHelloComplete();
 	connection.flush();
 }

wpilibc/wpilibC++/include/Utility.h

@@ -5,6 +5,10 @@
 /*---------------------------------------------------------------------------*/
 #pragma once
 
+/** @file
+ * Contains global utility functions
+ */
+
 #include <stdint.h>
 #include <string>
 

wpilibc/wpilibC++Devices/include/CameraServer.h

@@ -6,54 +6,76 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
+#include "USBCamera.h"
 #include "ErrorBase.h"
 #include "nivision.h"
 #include "NIIMAQdx.h"
 
-#include <vector>
-#include <thread>
 #include <mutex>
+#include <thread>
+#include <memory>
 #include <condition_variable>
+#include <tuple>
+#include <vector>
 
-/**
- * Class that runs a TCP server that serves an M-JPEG stream to the dashboard.
- */
 class CameraServer : public ErrorBase {
-	static constexpr uint16_t kPort = 1180;
-	static constexpr uint8_t kMagicNumber[] = { 0x01, 0x00, 0x00, 0x00 };
-	static constexpr uint32_t kSize640x480 = 0;
-	static constexpr uint32_t kSize320x240 = 1;
-	static constexpr uint32_t kSize160x120 = 2;
-	static constexpr int32_t kHardwareCompression = -1;
-	static constexpr char const *kDefaultCameraName = "cam0";
+ private:
+  static constexpr uint16_t kPort = 1180;
+  static constexpr uint8_t kMagicNumber[] = { 0x01, 0x00, 0x00, 0x00 };
+  static constexpr uint32_t kSize640x480 = 0;
+  static constexpr uint32_t kSize320x240 = 1;
+  static constexpr uint32_t kSize160x120 = 2;
+  static constexpr int32_t kHardwareCompression = -1;
+  static constexpr uint32_t kMaxImageSize = 200000;
 
-public:
-	static CameraServer *GetInstance();
+ protected:
+  CameraServer();
+ 
+  std::shared_ptr<USBCamera> m_camera;
+  std::thread m_serverThread;
+  std::thread m_captureThread;
+  std::recursive_mutex m_imageMutex;
+  std::condition_variable_any m_newImageVariable;
+  std::vector<uint8_t*> m_dataPool;
+  unsigned int m_quality;
+  bool m_autoCaptureStarted;
+  bool m_hwClient;
+  std::tuple<uint8_t*, unsigned int, unsigned int, bool> m_imageData;
 
-	void SetImage(Image const *image);
+  void Serve();
+  void AutoCapture();
+  void SetImageData(uint8_t* data, unsigned int size, unsigned int start = 0, bool imaqData = false);
+  void FreeImageData(std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData);
 
-	void StartAutomaticCapture(char const *cameraName = kDefaultCameraName);
+  struct Request {
+    uint32_t fps;
+    int32_t compression;
+    uint32_t size;
+  };
 
-	void SetQuality(unsigned int quality);
-	unsigned int GetQuality() const;
+  static CameraServer* s_instance;
+  
+ public:
+  static CameraServer* GetInstance();
+  void SetImage(Image const *image);
 
-protected:
-	CameraServer();
-	void serve();
+  void StartAutomaticCapture(char const *cameraName = USBCamera::kDefaultCameraName);
 
-	std::thread m_serverThread;
-	std::recursive_mutex m_imageMutex;
-	std::condition_variable_any m_newImageReady;
-	std::vector<uint8_t> m_imageData;
-	unsigned int m_quality;
-	bool m_autoCaptureStarted;
+  /**
+   * 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} should not be called after this is called.
+   *
+   * @param camera The camera interface (eg. USBCamera)
+   */
+  void StartAutomaticCapture(std::shared_ptr<USBCamera> camera);
 
-	struct Request {
-		uint32_t fps;
-		int32_t compression;
-		uint32_t size;
-	};
+  bool IsAutoCaptureStarted();
 
-	static CameraServer *s_instance;
+  void SetQuality(unsigned int quality);
+  unsigned int GetQuality();
+
+  void SetSize(unsigned int size);
 };
-

wpilibc/wpilibC++Devices/include/Encoder.h

@@ -29,6 +29,7 @@ class DigitalSource;
 class Encoder : public SensorBase, public CounterBase, public PIDSource, public LiveWindowSendable
 {
 public:
+	enum IndexingType { kResetWhileHigh, kResetWhileLow, kResetOnFallingEdge, kResetOnRisingEdge };
 
 	Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection = false,
 			EncodingType encodingType = k4X);
@@ -57,6 +58,10 @@ public:
 	void SetPIDSourceParameter(PIDSourceParameter pidSource);
 	double PIDGet();
 
+	void SetIndexSource(uint32_t channel, IndexingType type = kResetOnRisingEdge);
+	void SetIndexSource(DigitalSource *source, IndexingType type = kResetOnRisingEdge);
+	void SetIndexSource(DigitalSource &source, IndexingType type = kResetOnRisingEdge);
+
 	void UpdateTable();
 	void StartLiveWindowMode();
 	void StopLiveWindowMode();

wpilibc/wpilibC++Devices/include/PowerDistributionPanel.h

@@ -12,7 +12,8 @@
 #include "LiveWindow/LiveWindowSendable.h"
 
 /**
- * Class for getting voltage, current, and temperature from the CAN PDP
+ * Class for getting voltage, current, temperature, power and energy from the CAN PDP.
+ * The PDP must be at CAN Address 0.
  * @author Thomas Clark
  */
 class PowerDistributionPanel : public SensorBase, public LiveWindowSendable {

wpilibc/wpilibC++Devices/include/RobotDrive.h

@@ -67,6 +67,7 @@ public:
 	void SetInvertedMotor(MotorType motor, bool isInverted);
 	void SetSensitivity(float sensitivity);
 	void SetMaxOutput(double maxOutput);
+	void SetCANJaguarSyncGroup(uint8_t syncGroup);
 
 	void SetExpiration(float timeout);
 	float GetExpiration();
@@ -93,6 +94,7 @@ protected:
 	SpeedController *m_rearLeftMotor;
 	SpeedController *m_rearRightMotor;
 	MotorSafetyHelper *m_safetyHelper;
+	uint8_t m_syncGroup;
 
 private:
 	int32_t GetNumMotors()

wpilibc/wpilibC++Devices/include/USBCamera.h

@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "ErrorBase.h"
+#include "nivision.h"
+#include "NIIMAQdx.h"
+
+#include <mutex>
+#include <string>
+
+typedef enum whiteBalance_enum {
+  kFixedIndoor = 3000,
+  kFixedOutdoor1 = 4000,
+  kFixedOutdoor2 = 5000,
+  kFixedFluorescent1 = 5100,
+  kFixedFlourescent2 = 5200
+} whiteBalance;
+
+class USBCamera : public ErrorBase {
+ private:
+  static constexpr char const *ATTR_WB_MODE = "CameraAttributes::WhiteBalance::Mode";
+  static constexpr char const *ATTR_WB_VALUE = "CameraAttributes::WhiteBalance::Value";
+  static constexpr char const *ATTR_EX_MODE = "CameraAttributes::Exposure::Mode";
+  static constexpr char const *ATTR_EX_VALUE = "CameraAttributes::Exposure::Value";
+  static constexpr char const *ATTR_BR_MODE = "CameraAttributes::Brightness::Mode";
+  static constexpr char const *ATTR_BR_VALUE = "CameraAttributes::Brightness::Value";
+
+ protected:
+  IMAQdxSession m_id;
+  std::string m_name;
+  bool m_useJpeg;
+  bool m_active;
+  bool m_open;
+
+  std::recursive_mutex m_mutex;
+
+  unsigned int m_width;
+  unsigned int m_height;
+  double m_fps;
+  std::string m_whiteBalance;
+  unsigned int m_whiteBalanceValue;
+  bool m_whiteBalanceValuePresent;
+  std::string m_exposure;
+  unsigned int m_exposureValue;
+  bool m_exposureValuePresent;
+  unsigned int m_brightness;
+  bool m_needSettingsUpdate;
+
+  unsigned int GetJpegSize(void* buffer, unsigned int buffSize);
+
+ public:
+  static constexpr char const *kDefaultCameraName = "cam0";
+
+  USBCamera(std::string name, bool useJpeg);
+  
+  void OpenCamera();
+  void CloseCamera();
+  void StartCapture();
+  void StopCapture();
+  void SetFPS(double fps);
+  void SetSize(unsigned int width, unsigned int height);
+
+  void UpdateSettings();
+  /**
+   * Set the brightness, as a percentage (0-100).
+   */
+  void SetBrightness(unsigned int brightness);
+
+  /**
+   * Get the brightness, as a percentage (0-100).
+   */
+  unsigned int GetBrightness();
+
+  /**
+   * Set the white balance to auto
+   */
+  void SetWhiteBalanceAuto();
+
+  /**
+   * Set the white balance to hold current
+   */
+  void SetWhiteBalanceHoldCurrent();
+
+  /**
+   * Set the white balance to manual, with specified color temperature
+   */
+  void SetWhiteBalanceManual(unsigned int wbValue);
+
+  /**
+   * Set the exposure to auto exposure
+   */
+  void SetExposureAuto();
+
+  /**
+   * Set the exposure to hold current
+   */
+  void SetExposureHoldCurrent();
+
+  /**
+   * Set the exposure to manual, with a given percentage (0-100)
+   */
+  void SetExposureManual(unsigned int expValue);
+  
+  void GetImage(Image* image);
+  unsigned int GetImageData(void* buffer, unsigned int bufferSize);
+};

wpilibc/wpilibC++Devices/src/CANTalon.cpp

@@ -66,9 +66,16 @@ void CANTalon::PIDWrite(float output)
 	}
 }
 
-/**
- * TODO documentation (see CANJaguar.cpp)
- */
+  /**
+   * Gets the current status of the Talon (usually a sensor value).
+   *
+   * In Current mode: returns output current.
+   * In Speed mode: returns current speed.
+   * In Position mode: returns current sensor position.
+   * In PercentVbus and Follower modes: returns current applied throttle.
+   *
+   * @return The current sensor value of the Talon.
+   */
 float CANTalon::Get()
 {
   int value;
@@ -94,11 +101,12 @@ float CANTalon::Get()
  * Sets the appropriate output on the talon, depending on the mode.
  *
  * In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped.
- * In Voltage mode, outputValue is in volts.
- * In Current mode, outputValue is in amperes.
- * In Speed mode, outputValue is in position change / 10ms.
- * In Position mode, outputValue is in encoder ticks or an analog value,
+ * In Voltage mode, output value is in volts.
+ * In Current mode, output value is in amperes.
+ * In Speed mode, output value is in position change / 10ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
  *   depending on the sensor.
+ * In Follower mode, the output value is the integer device ID of the talon to duplicate.
  *
  * @param outputValue The setpoint value, as described above.
  * @see SelectProfileSlot to choose between the two sets of gains.
@@ -188,10 +196,12 @@ void CANTalon::SetP(double p)
 	}
 }
 
-/**
- * TODO documentation (see CANJaguar.cpp)
- * @see SelectProfileSlot to choose between the two sets of gains.
- */
+ /**
+  * Set the integration constant of the currently selected profile.
+  *
+  * @param i Integration constant for the currently selected PID profile.
+  * @see SelectProfileSlot to choose between the two sets of gains.
+  */
 void CANTalon::SetI(double i)
 {
   CTR_Code status = m_impl->SetIgain(m_profile, i);
@@ -201,7 +211,9 @@ void CANTalon::SetI(double i)
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * Set the derivative constant of the currently selected profile.
+ *
+ * @param d Derivative constant for the currently selected PID profile.
  * @see SelectProfileSlot to choose between the two sets of gains.
  */
 void CANTalon::SetD(double d)
@@ -212,7 +224,9 @@ void CANTalon::SetD(double d)
 	}
 }
 /**
+ * Set the feedforward value of the currently selected profile.
  *
+ * @param f Feedforward constant for the currently selected PID profile.
  * @see SelectProfileSlot to choose between the two sets of gains.
  */
 void CANTalon::SetF(double f)
@@ -248,7 +262,11 @@ void CANTalon::SelectProfileSlot(int slotIdx)
 	}
 }
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * Sets control values for closed loop control.
+ *
+ * @param p Proportional constant.
+ * @param i Integration constant.
+ * @param d Differential constant.
  * This function does not modify F-gain.  Considerable passing a zero for f using
  * the four-parameter function.
  */
@@ -258,6 +276,14 @@ void CANTalon::SetPID(double p, double i, double d)
 	SetI(i);
 	SetD(d);
 }
+/**
+ * Sets control values for closed loop control.
+ *
+ * @param p Proportional constant.
+ * @param i Integration constant.
+ * @param d Differential constant.
+ * @param f Feedforward constant.
+ */
 void CANTalon::SetPID(double p, double i, double d, double f)
 {
 	SetP(p);
@@ -287,7 +313,9 @@ void CANTalon::SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs)
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * Get the current proportional constant.
+ *
+ * @return double proportional constant for current profile.
  * @see SelectProfileSlot to choose between the two sets of gains.
  */
 double CANTalon::GetP()
@@ -402,7 +430,7 @@ double CANTalon::GetSetpoint() {
 /**
  * Returns the voltage coming in from the battery.
  *
- * @return The input voltage in vols.
+ * @return The input voltage in volts.
  */
 float CANTalon::GetBusVoltage()
 {
@@ -415,7 +443,7 @@ float CANTalon::GetBusVoltage()
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * @return The voltage being output by the Talon, in Volts.
  */
 float CANTalon::GetOutputVoltage()
 {
@@ -430,7 +458,7 @@ float CANTalon::GetOutputVoltage()
 
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ *  Returns the current going through the Talon, in Amperes.
  */
 float CANTalon::GetOutputCurrent()
 {
@@ -444,9 +472,9 @@ float CANTalon::GetOutputCurrent()
 	return current;
 }
 
-/**
- * TODO documentation (see CANJaguar.cpp)
- */
+  /**
+   *  Returns temperature of Talon, in degrees Celsius.
+   */
 float CANTalon::GetTemperature()
 {
   double temp;
@@ -965,7 +993,7 @@ int CANTalon::GetBrakeEnableDuringNeutral()
 	return brakeEn;
 }
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * @deprecated not implemented
  */
 void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev)
 {
@@ -973,7 +1001,7 @@ void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev)
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * @deprecated not implemented
  */
 void CANTalon::ConfigPotentiometerTurns(uint16_t turns)
 {
@@ -981,7 +1009,7 @@ void CANTalon::ConfigPotentiometerTurns(uint16_t turns)
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * @deprecated not implemented
  */
 void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
 {

wpilibc/wpilibC++Devices/src/CameraServer.cpp

@@ -1,21 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
 #include "CameraServer.h"
-#include "ErrorBase.h"
 #include "WPIErrors.h"
 #include "Utility.h"
-#include "Timer.h"
 
 #include <iostream>
-#include <algorithm>
-#include <algorithm>
+#include <chrono>
 #include <cstring>
-
 #include <sys/socket.h>
 #include <unistd.h>
 #include <netdb.h>
@@ -23,228 +12,234 @@
 constexpr uint8_t CameraServer::kMagicNumber[];
 CameraServer *CameraServer::s_instance = nullptr;
 
-/**
- * Singleton getter.
- */
-CameraServer *CameraServer::GetInstance() {
-	if(s_instance == nullptr) {
-		s_instance = new CameraServer;
-	}
-
-	return s_instance;
+CameraServer* CameraServer::GetInstance() {
+  if (s_instance == NULL) {
+    s_instance = new CameraServer;
+  }
+  return s_instance;
 }
 
 CameraServer::CameraServer() :
-		m_serverThread(&CameraServer::serve, this),
-		m_quality(50),
-		m_autoCaptureStarted(false) {
+  m_camera(),
+  m_serverThread(&CameraServer::Serve, this),
+  m_captureThread(),
+  m_imageMutex(),
+  m_newImageVariable(),
+  m_dataPool(3),
+  m_quality(50),
+  m_autoCaptureStarted(false),
+  m_hwClient(true),
+  m_imageData(nullptr, 0, 0, false) {
+  for (int i = 0; i < 3; i++)
+    m_dataPool.push_back(new uint8_t[kMaxImageSize]);
 }
 
-/**
- * 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
- */
-void CameraServer::SetImage(Image const *image) {std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
-
-	/* Flatten the IMAQ image to a JPEG */
-	uint32_t dataSize;
-	uint8_t *data = (uint8_t *)imaqFlatten(image, IMAQ_FLATTEN_IMAGE, IMAQ_COMPRESSION_JPEG, 10 * m_quality, &dataSize);
-
-	/* Find the start of the JPEG data */
-	uint8_t *jpegData = data;
-	while(jpegData[0] != 0xff || jpegData[1] != 0xd8) {
-		jpegData++;
-		dataSize--;
-
-		wpi_assert(dataSize >= 2);
-	}
-
-	m_imageData.assign(dataSize, '\0');
-	std::copy(jpegData, jpegData + dataSize, m_imageData.begin());
-
-	/* Release the data from IMAQ */
-	imaqDispose(data);
-
-	m_newImageReady.notify_all();
+void CameraServer::FreeImageData(std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData) {
+  if (std::get<3>(imageData)) imaqDispose(std::get<0>(imageData));
+  else if (std::get<0>(imageData) != nullptr) {
+    std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+    m_dataPool.push_back(std::get<0>(imageData));
+  }
+}
+
+void CameraServer::SetImageData(uint8_t* data, unsigned int size, unsigned int start, bool imaqData) {
+  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  FreeImageData(m_imageData);
+  m_imageData = std::make_tuple(data, size, start, imaqData);
+  m_newImageVariable.notify_all();
+}
+  
+void CameraServer::SetImage(Image const *image) {
+  unsigned int dataSize = 0;
+  uint8_t* data = (uint8_t*) imaqFlatten(image, IMAQ_FLATTEN_IMAGE, IMAQ_COMPRESSION_JPEG, 10 * m_quality, &dataSize);
+
+  // If we're using a HW camera, then find the start of the data
+  bool hwClient;
+  {
+    // Make a local copy of the hwClient variable so that we can safely use it.
+    std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+    hwClient = m_hwClient;
+  }
+  unsigned int start = 0;
+  if (hwClient) {
+    while (start < dataSize - 1) {
+      if (data[start] == 0xFF && data[start + 1] == 0xD8) break;
+      else start++;
+    }
+  }
+  dataSize -= start;
+
+  wpi_assert(dataSize > 2);
+  SetImageData(data, dataSize, start, true);
+}  
+
+void CameraServer::AutoCapture() {
+  Image* frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
+
+  while (true) {
+    bool hwClient;
+    uint8_t* data = nullptr;
+    {
+      std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+      hwClient = m_hwClient;
+      if (hwClient) {
+        data = m_dataPool.back();
+        m_dataPool.pop_back();
+      }
+    }
+
+    if (hwClient) {
+      unsigned int size = m_camera->GetImageData(data, kMaxImageSize);
+      SetImageData(data, size);
+    } else {
+      m_camera->GetImage(frame);
+      SetImage(frame);
+    }
+  }
 }
 
-/**
- * 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")
- */
 void CameraServer::StartAutomaticCapture(char const *cameraName) {
-	if(m_autoCaptureStarted) {
-		wpi_setWPIErrorWithContext(IncompatibleState, "Automatic capture has already been started");
-		return;
-	}
-
-	/* Capturing images takes a lot of CPU time, since JPEG compression is
-		done in software, so this is done in a new thread. */
-	std::thread captureThread([cameraName, this] {
-		IMAQdxSession session;
-		IMAQdxError error;
-		Image *frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
-
-		error = IMAQdxOpenCamera(cameraName, IMAQdxCameraControlModeController, &session);
-		if(error != IMAQdxErrorSuccess) {
-			wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera");
-		}
-
-		error = IMAQdxConfigureGrab(session);
-		if(error != IMAQdxErrorSuccess) {
-			wpi_setImaqErrorWithContext(error, "IMAQdxConfigureGrab");
-		}
-
-		error = IMAQdxStartAcquisition(session);
-		if(error != IMAQdxErrorSuccess) {
-			wpi_setImaqErrorWithContext(error, "IMAQdxStartAcquisition");
-		}
-
-		/* In an infinite loop, wait for an image from the camera, then sent
-			it to the dashboard. */
-		for(;;) {
-			error = IMAQdxGrab(session, frame, true, NULL);
-			if(error != IMAQdxErrorSuccess) {
-				wpi_setImaqErrorWithContext(error, "IMAQdxGrab");
-				break;
-			}
-
-			SetImage(frame);
-		}
-
-		/* If something went wrong, close the session */
-		IMAQdxStopAcquisition(session);
-		IMAQdxCloseCamera(session);
-	});
-
-	captureThread.detach();
+  std::shared_ptr<USBCamera> camera = std::make_shared<USBCamera>(cameraName, true);
+  camera->OpenCamera();
+  StartAutomaticCapture(camera);
+}
+
+void CameraServer::StartAutomaticCapture(std::shared_ptr<USBCamera> camera) {
+  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  if (m_autoCaptureStarted) return;
+
+  m_camera = camera;
+  m_camera->StartCapture();
+
+  m_captureThread = std::thread(&CameraServer::AutoCapture, this);
+  m_captureThread.detach();
+  m_autoCaptureStarted = true;
+}
+
+bool CameraServer::IsAutoCaptureStarted() {
+  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  return m_autoCaptureStarted;
+}
+
+void CameraServer::SetSize(unsigned int size) {
+  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  if (!m_camera) return;
+  if (size == kSize160x120) m_camera->SetSize(160, 120);
+  else if (size == kSize320x240) m_camera->SetSize(320, 240);
+  else if (size == kSize640x480) m_camera->SetSize(640, 480);
 }
 
-/**
- * Set the quality of the compressed image sent to the dashboard
- *
- * @param quality The quality of the JPEG image, from 0 to 100
- */
 void CameraServer::SetQuality(unsigned int quality) {
-	if(quality > 100) {
-		wpi_setWPIErrorWithContext(ParameterOutOfRange, "JPEG quality should be from 0 to 100");
-		return;
-	}
-
-	m_quality = quality;
+  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  m_quality = quality > 100 ? 100 : quality;
 }
 
-/**
- * Get the quality of the compressed image sent to the dashboard
- *
- * @return The quality, from 0 to 100
- */
-unsigned int CameraServer::GetQuality() const {
-	return m_quality;
+unsigned int CameraServer::GetQuality() {
+  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  return m_quality;
 }
 
-/**
- * Run the M-JPEG server.
- *
- * This function listens for a connection from the dashboard in a background
- * thread, then sends back the M-JPEG stream.
- */
-void CameraServer::serve() {
-	int sock = socket(AF_INET, SOCK_STREAM, 0);
+void CameraServer::Serve() {
+  int sock = socket(AF_INET, SOCK_STREAM, 0);
 
-	if(sock == -1) {
-		wpi_setErrnoError();
-	}
+  if (sock == -1)
+    wpi_setErrnoError();
 
-	int reuseAddr = 1;
-	if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr, sizeof(reuseAddr)) == -1) {
-		wpi_setErrnoError();
-	}
+  int reuseAddr = 1;
+  if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr, sizeof(reuseAddr)) == -1)
+    wpi_setErrnoError();
 
-	sockaddr_in address, clientAddress;
+  sockaddr_in address, clientAddress;
 
-	memset(&address, 0, sizeof(address));
-	address.sin_family = AF_INET;
-	address.sin_addr.s_addr = htonl(INADDR_ANY);
-	address.sin_port = htons(kPort);
+  memset(&address, 0, sizeof(address));
+  address.sin_family = AF_INET;
+  address.sin_addr.s_addr = htonl(INADDR_ANY);
+  address.sin_port = htons(kPort);
 
-	if(bind(sock, (struct sockaddr *)&address, sizeof(address)) == -1) {
-		wpi_setErrnoError();
-	}
+  if (bind(sock, (struct sockaddr *)&address, sizeof(address)) == -1)
+    wpi_setErrnoError();
 
-	if(listen(sock, 10) == -1) {
-		wpi_setErrnoError();
-	}
+  if (listen(sock, 10) == -1)
+    wpi_setErrnoError();
 
-	for(;;) {
-		socklen_t clientaddresslen = sizeof(clientAddress);
-		int conn = accept(sock, (struct sockaddr *)&clientAddress, &clientaddresslen);
+  while(true) {
+    socklen_t clientAddressLen = sizeof(clientAddress);
 
-		/* Read the request from the dashboard */
-		Request req;
-		if(read(conn, &req, sizeof req) == -1) {
-			wpi_setErrnoError();
-			close(conn);
-			continue;
-		} else {
-			req.fps = ntohl(req.fps);
-			req.compression = ntohl(req.compression);
-			req.size = ntohl(req.size);
-		}
+    int conn = accept(sock, (struct sockaddr*)&clientAddress, &clientAddressLen);
+    if (conn == -1) {
+      wpi_setErrnoError();
+      continue;
+    }
 
-		/* Only the "hardware compression" mode is supported from C++ */
-		if(req.compression != kHardwareCompression) {
-			wpi_setWPIErrorWithContext(IncompatibleState, "Choose \"USB Camera HW\" on the dashboard");
-			close(conn);
-			continue;
-		}
+    Request req;
+    if (read(conn, &req, sizeof(req)) == -1) {
+      wpi_setErrnoError();
+      close(conn);
+      continue;
+    } else {
+      req.fps = ntohl(req.fps);
+      req.compression = ntohl(req.compression);
+      req.size = ntohl(req.size);
+    }
 
-		/* The period that frames are sent is 1/FPS */
-		auto period = std::chrono::microseconds(1000000) / req.fps;
+    // TODO: Support the SW Compression. The rest of the code below will work as though this
+    // check isn't here
+    if (req.compression != kHardwareCompression) {
+      wpi_setWPIErrorWithContext(IncompatibleState, "Choose \"USB Camera HW\" on the dashboard");
+      close(conn);
+      continue;
+    }
 
-		for(;;) {
-			auto startTime = std::chrono::steady_clock::now();
+    {
+      // Wait for the camera to be setw
+      std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+      if (!m_camera) {
+        std::cout << "Camera not yet ready, awaiting first image" << std::endl;
+        m_newImageVariable.wait(lock);
+      }
+      m_hwClient = req.compression == kHardwareCompression;
+      if (!m_hwClient) SetQuality(100 - req.compression);
+      else if (m_camera) m_camera->SetFPS(req.fps);
+      SetSize(req.size);
+    }
 
-			{
-				std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+    auto period = std::chrono::microseconds(1000000) / req.fps;
+    while (true) {
+      auto startTime = std::chrono::steady_clock::now();
+      std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData;
+      {
+        std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+        m_newImageVariable.wait(lock);
+        imageData = m_imageData;
+        m_imageData = std::make_tuple(nullptr, 0, 0, false);
+      }
 
-				m_newImageReady.wait(lock);
+      unsigned int size = std::get<1>(imageData);
+      unsigned int netSize = htonl(size);
+      unsigned int start = std::get<2>(imageData);
+      uint8_t *data = std::get<0>(imageData);
 
-				/* Send the magic number that indicates the beginning of a new image */
-				if(write(conn, kMagicNumber, sizeof kMagicNumber) == -1) {
-					break;
-				}
+      if (data == nullptr) continue;
 
-				/* Send the size of this image */
-				uint32_t size = htonl(m_imageData.size());
-				if(write(conn, &size, sizeof size) == -1) {
-					m_imageMutex.unlock();
-					break;
-				}
-
-				/* Send the image data itself */
-				if(write(conn, m_imageData.data(), m_imageData.size()) == -1) {
-					break;
-				}
-			}
-
-			/* Sleep long enough to maintain a constant framerate */
-			std::this_thread::sleep_until(startTime + period);
-		}
-
-		close(conn);
-	}
-
-	close(sock);
+      if (write(conn, kMagicNumber, sizeof(kMagicNumber)) == -1) {
+        wpi_setErrnoErrorWithContext("[CameraServer] Error sending magic number");
+        FreeImageData(imageData);
+        break;
+      }
+      if (write(conn, &netSize, sizeof(netSize)) == -1) {
+        wpi_setErrnoErrorWithContext("[CameraServer] Error sending image size");
+        FreeImageData(imageData);
+        break;
+      }
+      if (write(conn, &data[start], sizeof(uint8_t) * size) == -1) {
+        wpi_setErrnoErrorWithContext("[CameraServer] Error sending image data");
+        FreeImageData(imageData);
+        break;
+      }
+      FreeImageData(imageData);
+      std::this_thread::sleep_until(startTime + period);
+    }
+    close(conn);
+  }
+  close(sock);
 }

wpilibc/wpilibC++Devices/src/DoubleSolenoid.cpp

@@ -60,8 +60,8 @@ void DoubleSolenoid::InitSolenoid()
 /**
  * Constructor.
  * Uses the default PCM ID of 0
- * @param forwardChannel The forward channel number on the PCM.
- * @param reverseChannel The reverse channel number on the PCM.
+ * @param forwardChannel The forward channel number on the PCM (0..7).
+ * @param reverseChannel The reverse channel number on the PCM (0..7).
  */
 DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
 	: SolenoidBase (GetDefaultSolenoidModule())
@@ -75,8 +75,8 @@ DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
  * Constructor.
  *
  * @param moduleNumber The CAN ID of the PCM.
- * @param forwardChannel The forward channel on the module to control.
- * @param reverseChannel The reverse channel on the module to control.
+ * @param forwardChannel The forward channel on the PCM to control (0..7).
+ * @param reverseChannel The reverse channel on the PCM to control (0..7).
  */
 DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel)
 	: SolenoidBase (moduleNumber)

wpilibc/wpilibC++Devices/src/DriverStation.cpp

@@ -270,7 +270,7 @@ int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
 	if (stick >= kJoystickPorts)
 	{
 		wpi_setWPIError(BadJoystickIndex);
-		return 0;
+		return -1;
 	}
 
 	if (pov >= m_joystickPOVs[stick].count)
@@ -279,7 +279,7 @@ int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
 			wpi_setWPIError(BadJoystickAxis);
 		else
 			ReportJoystickUnpluggedError("WARNING: Joystick POV missing, check if all controllers are plugged in\n");
-		return 0;
+		return -1;
 	}
 
 	return m_joystickPOVs[stick].povs[pov];
@@ -530,5 +530,11 @@ double DriverStation::GetMatchTime()
 void DriverStation::ReportError(std::string error)
 {
 	std::cout << error << std::endl;
-    HALSetErrorData(error.c_str(), error.size(), 0);
+	
+	HALControlWord controlWord;
+	HALGetControlWord(&controlWord);
+	if(controlWord.dsAttached)
+	{
+		HALSetErrorData(error.c_str(), error.size(), 0);
+	}
 }

wpilibc/wpilibC++Devices/src/Encoder.cpp

@@ -507,6 +507,47 @@ double Encoder::PIDGet()
 	}
 }
 
+/**
+ * Set the index source for the encoder.  When this source is activated, the encoder count automatically resets.
+ *
+ * @param channel A DIO channel to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(uint32_t channel, Encoder::IndexingType type) {
+	int32_t status = 0;
+	bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge);
+	bool edgeSensitive = (type == kResetOnFallingEdge) || (type == kResetOnRisingEdge);
+
+  setEncoderIndexSource(m_encoder, channel, false, activeHigh, edgeSensitive, &status);
+  wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the index source for the encoder.  When this source is activated, the encoder count automatically resets.
+ *
+ * @param channel A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(DigitalSource *source, Encoder::IndexingType type) {
+	int32_t status = 0;
+	bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge);
+	bool edgeSensitive = (type == kResetOnFallingEdge) || (type == kResetOnRisingEdge);
+
+  setEncoderIndexSource(m_encoder, source->GetChannelForRouting(), source->GetAnalogTriggerForRouting(), activeHigh,
+  		edgeSensitive, &status);
+  wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the index source for the encoder.  When this source is activated, the encoder count automatically resets.
+ *
+ * @param channel A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(DigitalSource &source, Encoder::IndexingType type) {
+	SetIndexSource(&source, type);
+}
+
 void Encoder::UpdateTable() {
 	if (m_table != NULL) {
         m_table->PutNumber("Speed", GetRate());

wpilibc/wpilibC++Devices/src/PowerDistributionPanel.cpp

@@ -18,7 +18,7 @@ PowerDistributionPanel::PowerDistributionPanel() {
 
 /**
  * Query the input voltage of the PDP
- * @return The voltage of the PDP
+ * @return The voltage of the PDP in volts
  */
 double
 PowerDistributionPanel::GetVoltage() {
@@ -93,7 +93,7 @@ PowerDistributionPanel::GetTotalCurrent() {
 
 /**
  * Query the total power drawn from the monitored PDP channels
- * @return The the total power drawn from the PDP channels in Joules
+ * @return The the total power drawn from the PDP channels in Watts
  */
 double
 PowerDistributionPanel::GetTotalPower() {
@@ -110,7 +110,7 @@ PowerDistributionPanel::GetTotalPower() {
 
 /**
  * Query the total energy drawn from the monitored PDP channels
- * @return The the total energy drawn from the PDP channels in Watts
+ * @return The the total energy drawn from the PDP channels in Joules
  */
 double
 PowerDistributionPanel::GetTotalEnergy() {

wpilibc/wpilibC++Devices/src/Relay.cpp

@@ -65,7 +65,7 @@ void Relay::InitRelay()
 
 /**
  * Relay constructor given a channel.
- * @param channel The channel number.
+ * @param channel The channel number (0-3).
  * @param direction The direction that the Relay object will control.
  */
 Relay::Relay(uint32_t channel, Relay::Direction direction)

wpilibc/wpilibC++Devices/src/RobotBase.cpp

@@ -65,7 +65,7 @@ RobotBase::RobotBase()
 	FILE *file = NULL;
 	file = fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
 
-	fputs("2015 C++ 1.0.0", file);
+	fputs("2015 C++ 1.2.0", file);
 	if (file != NULL)
 	fclose(file);
 }

wpilibc/wpilibC++Devices/src/RobotDrive.cpp

@@ -41,6 +41,7 @@ void RobotDrive::InitRobotDrive() {
 	m_maxOutput = 1.0;
 	m_safetyHelper = new MotorSafetyHelper(this);
 	m_safetyHelper->SetSafetyEnabled(true);
+	m_syncGroup = 0;
 }
 
 /** 
@@ -507,14 +508,15 @@ void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float
 
 	Normalize(wheelSpeeds);
 
-	uint8_t syncGroup = 0x80;
+	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup);
+	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup);
+	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup);
+	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup);
 
-	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
-	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
-	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
-	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
-
-	CANJaguar::UpdateSyncGroup(syncGroup);
+	if (m_syncGroup != 0)
+	{
+		CANJaguar::UpdateSyncGroup(m_syncGroup);
+	}
 
 	m_safetyHelper->Feed();
 }
@@ -556,14 +558,15 @@ void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rota
 
 	Normalize(wheelSpeeds);
 
-	uint8_t syncGroup = 0x80;
+	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup);
+	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup);
+	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup);
+	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup);
 
-	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
-	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
-	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
-	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
-
-	CANJaguar::UpdateSyncGroup(syncGroup);
+	if (m_syncGroup != 0)
+	{
+		CANJaguar::UpdateSyncGroup(m_syncGroup);
+	}
 
 	m_safetyHelper->Feed();
 }
@@ -595,17 +598,18 @@ void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput)
 {
 	wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL);
 
-	uint8_t syncGroup = 0x80;
-
 	if (m_frontLeftMotor != NULL)
-		m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
-	m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+		m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup);
+	m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup);
 
 	if (m_frontRightMotor != NULL)
-		m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
-	m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+		m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup);
+	m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup);
 
-	CANJaguar::UpdateSyncGroup(syncGroup);
+	if (m_syncGroup != 0)
+	{
+		CANJaguar::UpdateSyncGroup(m_syncGroup);
+	}
 
 	m_safetyHelper->Feed();
 }
@@ -698,7 +702,15 @@ void RobotDrive::SetMaxOutput(double maxOutput)
 	m_maxOutput = maxOutput;
 }
 
-
+/**
+ * Set the number of the sync group for the motor controllers.  If the motor controllers are {@link CANJaguar}s,
+ * then they will all be added to this sync group, causing them to update their values at the same time.
+ *
+ * @param syncGroup the update group to add the motor controllers to
+ */
+void RobotDrive::SetCANJaguarSyncGroup(uint8_t syncGroup) {
+	m_syncGroup = syncGroup;
+}
 
 void RobotDrive::SetExpiration(float timeout)
 {

wpilibc/wpilibC++Devices/src/USBCamera.cpp

@@ -0,0 +1,328 @@
+#include "USBCamera.h"
+
+#include "Utility.h"
+
+#include <regex>
+#include <chrono>
+#include <thread>
+#include <iostream>
+#include <iomanip>
+
+// This macro expands the given imaq function to ensure that it is called and
+// properly checked for an error, calling the wpi_setImaqErrorWithContext
+// macro
+// To call it, just give the name of the function and the arguments
+#define SAFE_IMAQ_CALL(funName, ...) { \
+    unsigned int error = funName(__VA_ARGS__); \
+    if (error != IMAQdxErrorSuccess) \
+      wpi_setImaqErrorWithContext(error, #funName); \
+  }
+
+// Constants for the manual and auto types
+static const std::string AUTO = "Auto";
+static const std::string MANUAL = "Manual";
+
+/**
+ * Helper function to determine the size of a jpeg. The general structure of
+ * how to parse a jpeg for length can be found in this stackoverflow article:
+ * http://stackoverflow.com/a/1602428. Be sure to also read the comments for
+ * the SOS flag explanation.
+ */
+unsigned int USBCamera::GetJpegSize(void* buffer, unsigned int buffSize) {
+  uint8_t* data = (uint8_t*) buffer;
+  if (!wpi_assert(data[0] == 0xff && data[1] == 0xd8)) return 0;
+  unsigned int pos = 2;
+  while (pos < buffSize) {
+    // All control markers start with 0xff, so if this isn't present,
+    // the JPEG is not valid
+    if (!wpi_assert(data[pos] == 0xff)) return 0;
+    unsigned char t = data[pos+1];
+    // These are RST markers. We just skip them and move onto the next marker
+    if (t == 0x01 || (t >= 0xd0 && t <= 0xd7)) {
+      pos += 2;
+    } else if (t == 0xd9) {
+      // End of Image, add 2 for this and 0-indexed
+      return pos + 2;
+    } else if (!wpi_assert(t != 0xd8))  {
+      // Another start of image, invalid image
+      return 0;
+    } else if (t == 0xda) {
+      // SOS marker. The next two bytes are a 16-bit big-endian int that is
+      // the length of the SOS header, skip that
+      unsigned int len = (((unsigned int) (data[pos+2] & 0xff)) << 8 | ((unsigned int) data[pos+3] & 0xff));
+      pos += len + 2;
+      // The next marker is the first marker that is 0xff followed by a non-RST
+      // element. 0xff followed by 0x00 is an escaped 0xff. 0xd0-0xd7 are RST
+      // markers
+      while (data[pos] != 0xff || data[pos+1] == 0x00 || (data[pos+1] >= 0xd0 && data[pos+1] <= 0xd7)) {
+        pos += 1;
+        if (pos >= buffSize) return 0;
+      }
+    } else {
+      // This is one of several possible markers. The next two bytes are a 16-bit
+      // big-endian int with the length of the marker header, skip that then
+      // continue searching
+      unsigned int len = (((unsigned int) (data[pos+2] & 0xff)) << 8 | ((unsigned int) data[pos+3] & 0xff));
+      pos += len + 2;
+    }
+  }
+
+  return 0;
+}
+
+USBCamera::USBCamera(std::string name, bool useJpeg) :
+  m_id(0),
+  m_name(name),
+  m_useJpeg(useJpeg),
+  m_active(false),
+  m_open(false),
+  m_mutex(),
+  m_width(320),
+  m_height(240),
+  m_fps(30),
+  m_whiteBalance(AUTO),
+  m_whiteBalanceValue(0),
+  m_whiteBalanceValuePresent(false),
+  m_exposure(MANUAL),
+  m_exposureValue(50),
+  m_exposureValuePresent(false),
+  m_brightness(80),
+  m_needSettingsUpdate(true) {
+}
+
+void USBCamera::OpenCamera() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  for (unsigned int i = 0; i < 3; i++) {
+    uInt32 id = 0;
+    // Can't use SAFE_IMAQ_CALL here because we only error on the third time
+    IMAQdxError error = IMAQdxOpenCamera(m_name.c_str(), IMAQdxCameraControlModeController, &id);
+    if (error != IMAQdxErrorSuccess) {
+      // Only error on the 3rd try
+      if (i >= 2)
+        wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera");
+      // Sleep for a few seconds to ensure the error has been dealt with
+      std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+    } else {
+      m_id = id;
+      m_open = true;
+      return;
+    }
+  }
+}
+
+void USBCamera::CloseCamera() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (!m_open) return;
+  SAFE_IMAQ_CALL(IMAQdxCloseCamera, m_id);
+  m_id = 0;
+  m_open = false;
+}
+
+void USBCamera::StartCapture() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (!m_open || m_active) return;
+  SAFE_IMAQ_CALL(IMAQdxConfigureGrab, m_id);
+  SAFE_IMAQ_CALL(IMAQdxStartAcquisition, m_id);
+  m_active = true;
+}
+
+void USBCamera::StopCapture() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (!m_open || !m_active) return;
+  SAFE_IMAQ_CALL(IMAQdxStopAcquisition, m_id);
+  SAFE_IMAQ_CALL(IMAQdxUnconfigureAcquisition, m_id);
+  m_active = false;
+}
+
+void USBCamera::UpdateSettings() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  bool wasActive = m_active;
+
+  if (wasActive)
+    StopCapture();
+  if (m_open)
+    CloseCamera();
+  OpenCamera();
+
+  uInt32 count = 0;
+  uInt32 currentMode = 0;
+  SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, NULL, &count, &currentMode);
+  IMAQdxVideoMode modes[count];
+  SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes, &count, &currentMode);
+
+  // Groups are:
+  //   0 - width
+  //   1 - height
+  //   2 - format
+  //   3 - fps
+  std::regex reMode("([0-9]+)\\s*x\\s*([0-9]+)\\s+(.*?)\\s+([0-9.]+)\\s*fps");
+  IMAQdxVideoMode* foundMode = nullptr;
+  IMAQdxVideoMode* currentModePtr = &modes[currentMode];
+  double foundFps = 1000.0;
+
+  // Loop through the modes, and find the match with the lowest fps
+  for (unsigned int i = 0; i < count; i++) {
+    std::cmatch m;
+    if (!std::regex_match(modes[i].Name, m, reMode))
+      continue;
+    unsigned int width = (unsigned int) std::stoul(m[1].str());
+    unsigned int height = (unsigned int) std::stoul(m[2].str());
+    if (width != m_width)
+      continue;
+    if (height != m_height)
+      continue;
+    double fps = atof(m[4].str().c_str());
+    if (fps < m_fps)
+      continue;
+    if (fps > foundFps)
+      continue;
+    bool isJpeg = m[3].str().compare("jpeg") == 0 || m[3].str().compare("JPEG") == 0;
+    if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg))
+      continue;
+    foundMode = &modes[i];
+    foundFps = fps;
+  }
+  if (foundMode != nullptr) {
+    if (foundMode->Value != currentModePtr->Value) {
+      SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, IMAQdxAttributeVideoMode, IMAQdxValueTypeU32, foundMode->Value);
+    }
+  } 
+
+  if (m_whiteBalance.compare(AUTO) == 0) {
+    SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, IMAQdxValueTypeString, AUTO.c_str());
+  } else {
+    SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, IMAQdxValueTypeString, MANUAL.c_str());
+    if (m_whiteBalanceValuePresent)
+      SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_VALUE, IMAQdxValueTypeU32, m_whiteBalanceValue);
+  }
+
+  if (m_exposure.compare(AUTO) == 0) {
+    SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE, IMAQdxValueTypeString, std::string("AutoAperaturePriority").c_str());
+  } else {
+    SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE, IMAQdxValueTypeString, MANUAL.c_str());
+    if (m_exposureValuePresent) {
+      double minv = 0.0;
+      double maxv = 0.0;
+      SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_EX_VALUE, IMAQdxValueTypeF64, &minv);
+      SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_EX_VALUE, IMAQdxValueTypeF64, &maxv);
+      double val = minv + ((maxv - minv) * ((double) m_exposureValue / 100.0));
+      SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_VALUE, IMAQdxValueTypeF64, val);
+    }
+  }
+
+  SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_MODE, IMAQdxValueTypeString, MANUAL.c_str());
+  double minv = 0.0;
+  double maxv = 0.0;
+  SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, &minv);
+  SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, &maxv);
+  double val = minv + ((maxv - minv) * ((double) m_brightness / 100.0));
+  SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, val);
+
+  if (wasActive)
+    StartCapture();
+}
+
+void USBCamera::SetFPS(double fps) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (m_fps != fps) {
+    m_needSettingsUpdate = true;
+    m_fps = fps;
+  }
+}
+
+void USBCamera::SetSize(unsigned int width, unsigned int height) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (m_width != width || m_height != height) {
+    m_needSettingsUpdate = true;
+    m_width = width;
+    m_height = height;
+  }
+}
+
+void USBCamera::SetBrightness(unsigned int brightness) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (m_brightness != brightness) {
+    m_needSettingsUpdate = true;
+    m_brightness = brightness;
+  }
+}
+
+unsigned int USBCamera::GetBrightness() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  return m_brightness;
+}
+
+void USBCamera::SetWhiteBalanceAuto() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  m_whiteBalance = AUTO;
+  m_whiteBalanceValue = 0;
+  m_whiteBalanceValuePresent = false;
+  m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetWhiteBalanceHoldCurrent() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  m_whiteBalance = MANUAL;
+  m_whiteBalanceValue = 0;
+  m_whiteBalanceValuePresent = false;
+  m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  m_whiteBalance = MANUAL;
+  m_whiteBalanceValue = whiteBalance;
+  m_whiteBalanceValuePresent = true;
+  m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureAuto() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  m_exposure = AUTO;
+  m_exposureValue = 0;
+  m_exposureValuePresent = false;
+  m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureHoldCurrent() {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  m_exposure = MANUAL;
+  m_exposureValue = 0;
+  m_exposureValuePresent = false;
+  m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureManual(unsigned int level) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  m_exposure = MANUAL;
+  if (level > 100) m_exposureValue = 100;
+  else m_exposureValue = level;
+  m_exposureValuePresent = true;
+  m_needSettingsUpdate = true;
+}
+
+void USBCamera::GetImage(Image* image) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (m_needSettingsUpdate || m_useJpeg) {
+    m_needSettingsUpdate = false;
+    m_useJpeg = false;
+    UpdateSettings();
+  }
+  // BufNum is not actually used for anything at our level, since we are
+  // waiting until the next image is ready anyway
+  uInt32 bufNum;
+  SAFE_IMAQ_CALL(IMAQdxGrab, m_id, image, 1, &bufNum);
+}
+
+unsigned int USBCamera::GetImageData(void* buffer, unsigned int bufferSize) {
+  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  if (m_needSettingsUpdate || !m_useJpeg) {
+    m_needSettingsUpdate = false;
+    m_useJpeg = true;
+    UpdateSettings();
+  }
+  // BufNum is not actually used for anything at our level
+  uInt32 bufNum;
+  SAFE_IMAQ_CALL(IMAQdxGetImageData, m_id, buffer, bufferSize, IMAQdxBufferNumberModeLast, 0, &bufNum);
+  return GetJpegSize(buffer, bufferSize);
+}

wpilibc/wpilibC++Devices/src/Vision/BaeUtilities.cpp

@@ -16,7 +16,7 @@
 #include "Servo.h"
 #include "Timer.h"
 
-/**
+/** @file
  *   Utility functions 
  */
 

wpilibc/wpilibC++Devices/src/Vision/VisionAPI.cpp

@@ -14,7 +14,9 @@
 int VisionAPI_debugFlag = 1;
 #define DPRINTF if(VisionAPI_debugFlag)dprintf
 
-/*   Image Management functions    */
+/** @file
+ *    Image Management functions
+ */
 
 /**
 * @brief Create an image object

wpilibc/wpilibC++IntegrationTests/src/FakeEncoderTest.cpp

@@ -9,24 +9,62 @@
 #include "gtest/gtest.h"
 #include "TestBench.h"
 
-static const double kDelayTime = 0.01;
+static const double kDelayTime = 0.001;
 
 class FakeEncoderTest : public testing::Test {
 protected:
-	Encoder *m_encoder;
 	DigitalOutput *m_outputA;
 	DigitalOutput *m_outputB;
+	AnalogOutput *m_indexOutput;
+
+	Encoder *m_encoder;
+	AnalogTrigger *m_indexAnalogTrigger;
+	AnalogTriggerOutput *m_indexAnalogTriggerOutput;
 
 	virtual void SetUp() {
 		m_outputA = new DigitalOutput(TestBench::kLoop2OutputChannel);
 		m_outputB = new DigitalOutput(TestBench::kLoop1OutputChannel);
+		m_indexOutput = new AnalogOutput(TestBench::kAnalogOutputChannel);
+
 		m_encoder = new Encoder(TestBench::kLoop1InputChannel, TestBench::kLoop2InputChannel);
+		m_indexAnalogTrigger = new AnalogTrigger(TestBench::kFakeAnalogOutputChannel);
+		m_indexAnalogTrigger->SetLimitsVoltage(2.0, 3.0);
+		m_indexAnalogTriggerOutput = m_indexAnalogTrigger->CreateOutput(AnalogTriggerType::kState);
 	}
 
 	virtual void TearDown() {
-		delete m_encoder;
 		delete m_outputA;
 		delete m_outputB;
+		delete m_indexOutput;
+		delete m_encoder;
+		delete m_indexAnalogTrigger;
+		delete m_indexAnalogTriggerOutput;
+	}
+
+	/**
+	 * Output pulses to the encoder's input channels to simulate a change of 100 ticks
+	 */
+	void Simulate100QuadratureTicks() {
+		for(int i = 0; i < 100; i++) {
+			m_outputA->Set(true);
+			Wait(kDelayTime);
+			m_outputB->Set(true);
+			Wait(kDelayTime);
+			m_outputA->Set(false);
+			Wait(kDelayTime);
+			m_outputB->Set(false);
+			Wait(kDelayTime);
+		}
+	}
+
+	void SetIndexHigh() {
+		m_indexOutput->SetVoltage(5.0);
+		Wait(kDelayTime);
+	}
+
+	void SetIndexLow() {
+		m_indexOutput->SetVoltage(0.0);
+		Wait(kDelayTime);
 	}
 };
 
@@ -42,22 +80,69 @@ TEST_F(FakeEncoderTest, TestDefaultState) {
  * Test the encoder by setting the digital outputs and reading the value.
  */
 TEST_F(FakeEncoderTest, TestCountUp) {
-	m_outputA->Set(false);
-	m_outputB->Set(false);
-	m_encoder->Reset();
-
-	//Sets the outputs such that the encoder moves in the positive direction
-	for(int i = 0; i < 100; i++) {
-		m_outputA->Set(true);
-		Wait(kDelayTime);
-		m_outputB->Set(true);
-		Wait(kDelayTime);
-		m_outputA->Set(false);
-		Wait(kDelayTime);
-		m_outputB->Set(false);
-		Wait(kDelayTime);
-	}
+	Simulate100QuadratureTicks();
 
 	EXPECT_FLOAT_EQ(100.0f, m_encoder->Get())
 		<< "Encoder did not count to 100.";
 }
+
+/**
+ * Test that the encoder can stay reset while the index source is high
+ */
+TEST_F(FakeEncoderTest, TestResetWhileHigh) {
+	m_encoder->SetIndexSource(m_indexAnalogTriggerOutput, Encoder::IndexingType::kResetWhileHigh);
+
+	SetIndexLow();
+	Simulate100QuadratureTicks();
+	SetIndexHigh();
+	EXPECT_EQ(0, m_encoder->Get());
+
+	Simulate100QuadratureTicks();
+	EXPECT_EQ(0, m_encoder->Get());
+}
+
+/**
+ * Test that the encoder can reset when the index source goes from low to high
+ */
+TEST_F(FakeEncoderTest, TestResetOnRisingEdge) {
+	m_encoder->SetIndexSource(m_indexAnalogTriggerOutput, Encoder::IndexingType::kResetOnRisingEdge);
+
+	SetIndexLow();
+	Simulate100QuadratureTicks();
+	SetIndexHigh();
+	EXPECT_EQ(0, m_encoder->Get());
+
+	Simulate100QuadratureTicks();
+	EXPECT_EQ(100, m_encoder->Get());
+}
+
+/**
+ * Test that the encoder can stay reset while the index source is low
+ */
+TEST_F(FakeEncoderTest, TestResetWhileLow) {
+	m_encoder->SetIndexSource(m_indexAnalogTriggerOutput, Encoder::IndexingType::kResetWhileLow);
+
+	SetIndexHigh();
+	Simulate100QuadratureTicks();
+	SetIndexLow();
+	EXPECT_EQ(0, m_encoder->Get());
+
+	Simulate100QuadratureTicks();
+	EXPECT_EQ(0, m_encoder->Get());
+}
+
+/**
+ * Test that the encoder can reset when the index source goes from high to low
+ */
+TEST_F(FakeEncoderTest, TestResetOnFallingEdge) {
+	m_encoder->SetIndexSource(m_indexAnalogTriggerOutput, Encoder::IndexingType::kResetOnFallingEdge);
+
+	SetIndexHigh();
+	Simulate100QuadratureTicks();
+	SetIndexLow();
+	EXPECT_EQ(0, m_encoder->Get());
+
+	Simulate100QuadratureTicks();
+	EXPECT_EQ(100, m_encoder->Get());
+}
+

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java

@@ -7,16 +7,18 @@
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.IntBuffer;
-
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
 import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
 import edu.wpi.first.wpilibj.communication.UsageReporting;
 import edu.wpi.first.wpilibj.hal.AnalogJNI;
 import edu.wpi.first.wpilibj.hal.HALUtil;
 import edu.wpi.first.wpilibj.util.BoundaryException;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.IntBuffer;
+
 //import com.sun.jna.Pointer;
-import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
 
 /**
  * Class for creating and configuring Analog Triggers
@@ -56,12 +58,14 @@ public class AnalogTrigger {
 	 */
 	protected void initTrigger(final int channel) {
 		ByteBuffer port_pointer = AnalogJNI.getPort((byte) channel);
-		IntBuffer index = ByteBuffer.allocateDirect(4).asIntBuffer();
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
+		ByteBuffer index = ByteBuffer.allocateDirect(4);
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		index.order(ByteOrder.LITTLE_ENDIAN);
+		status.order(ByteOrder.LITTLE_ENDIAN);
 
-		m_port = AnalogJNI.initializeAnalogTrigger(port_pointer, index, status);
-		HALUtil.checkStatus(status);
-		m_index = index.get(0);
+		m_port = AnalogJNI.initializeAnalogTrigger(port_pointer, index.asIntBuffer(), status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
+		m_index = index.asIntBuffer().get(0);
 
 		UsageReporting.report(tResourceType.kResourceType_AnalogTrigger, channel);
 	}
@@ -95,9 +99,10 @@ public class AnalogTrigger {
 	 * Release the resources used by this object
 	 */
 	public void free() {
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
-		AnalogJNI.cleanAnalogTrigger(m_port, status);
-		HALUtil.checkStatus(status);
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
+		AnalogJNI.cleanAnalogTrigger(m_port, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 		m_port = null;
 	}
 
@@ -115,9 +120,10 @@ public class AnalogTrigger {
 		if (lower > upper) {
 			throw new BoundaryException("Lower bound is greater than upper");
 		}
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
-		AnalogJNI.setAnalogTriggerLimitsRaw(m_port, lower, upper, status);
-		HALUtil.checkStatus(status);
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
+		AnalogJNI.setAnalogTriggerLimitsRaw(m_port, lower, upper, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 	}
 
 	/**
@@ -134,10 +140,11 @@ public class AnalogTrigger {
 			throw new BoundaryException(
 					"Lower bound is greater than upper bound");
 		}
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
 		AnalogJNI.setAnalogTriggerLimitsVoltage(m_port, (float) lower,
-				(float) upper, status);
-		HALUtil.checkStatus(status);
+				(float) upper, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 	}
 
 	/**
@@ -149,10 +156,11 @@ public class AnalogTrigger {
 	 *            true to use an averaged value, false otherwise
 	 */
 	public void setAveraged(boolean useAveragedValue) {
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
 		AnalogJNI.setAnalogTriggerAveraged(m_port,
-				(byte) (useAveragedValue ? 1 : 0), status);
-		HALUtil.checkStatus(status);
+				(byte) (useAveragedValue ? 1 : 0), status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 	}
 
 	/**
@@ -165,10 +173,11 @@ public class AnalogTrigger {
 	 *            true to use a filterd value, false otherwise
 	 */
 	public void setFiltered(boolean useFilteredValue) {
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
 		AnalogJNI.setAnalogTriggerFiltered(m_port,
-				(byte) (useFilteredValue ? 1 : 0), status);
-		HALUtil.checkStatus(status);
+				(byte) (useFilteredValue ? 1 : 0), status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 	}
 
 	/**
@@ -188,9 +197,10 @@ public class AnalogTrigger {
 	 * @return The InWindow output of the analog trigger.
 	 */
 	public boolean getInWindow() {
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
-		byte value = AnalogJNI.getAnalogTriggerInWindow(m_port, status);
-		HALUtil.checkStatus(status);
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
+		byte value = AnalogJNI.getAnalogTriggerInWindow(m_port, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 		return value != 0;
 	}
 
@@ -202,9 +212,10 @@ public class AnalogTrigger {
 	 * @return The TriggerState output of the analog trigger.
 	 */
 	public boolean getTriggerState() {
-		IntBuffer status = ByteBuffer.allocateDirect(4).asIntBuffer();
-		byte value = AnalogJNI.getAnalogTriggerTriggerState(m_port, status);
-		HALUtil.checkStatus(status);
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
+		byte value = AnalogJNI.getAnalogTriggerTriggerState(m_port, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
 		return value != 0;
 	}
 
@@ -217,7 +228,7 @@ public class AnalogTrigger {
 	 *            An enum of the type of output object to create.
 	 * @return A pointer to a new AnalogTriggerOutput object.
 	 */
-	AnalogTriggerOutput createOutput(AnalogTriggerType type) {
+	public AnalogTriggerOutput createOutput(AnalogTriggerType type) {
 		return new AnalogTriggerOutput(this, type);
 	}
 }

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java

@@ -7,13 +7,13 @@
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.IntBuffer;
-
 import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
 import edu.wpi.first.wpilibj.communication.UsageReporting;
 import edu.wpi.first.wpilibj.hal.AnalogJNI;
 import edu.wpi.first.wpilibj.hal.HALUtil;
 
+import java.nio.IntBuffer;
+
 /**
  * Class to represent a specific output from an analog trigger. This class is
  * used to get the current output value and also as a DigitalSource to provide
@@ -128,10 +128,10 @@ public class AnalogTriggerOutput extends DigitalSource {
 	 * @author jonathanleitschuh
 	 */
 	public enum AnalogTriggerType{
-		IN_WINDOW(AnalogJNI.AnalogTriggerType.kInWindow),
-		STATE(AnalogJNI.AnalogTriggerType.kState),
-		RISING_PULSE(AnalogJNI.AnalogTriggerType.kRisingPulse),
-		FALLING_PULSE(AnalogJNI.AnalogTriggerType.kFallingPulse);
+		kInWindow(AnalogJNI.AnalogTriggerType.kInWindow),
+		kState(AnalogJNI.AnalogTriggerType.kState),
+		kRisingPulse(AnalogJNI.AnalogTriggerType.kRisingPulse),
+		kFallingPulse(AnalogJNI.AnalogTriggerType.kFallingPulse);
 		
 		private final int value;
 		

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java

@@ -202,8 +202,8 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
    *
    * In Current mode: returns output current.
    * In Speed mode: returns current speed.
-   * In Position omde: returns current sensor position.
-   * In Throttle and Follower modes: returns current applied throttle.
+   * In Position mode: returns current sensor position.
+   * In PercentVbus and Follower modes: returns current applied throttle.
    *
    * @return The current sensor value of the Talon.
    */
@@ -358,14 +358,18 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
     m_impl.GetBrakeIsEnabled(swigp);
     return (CanTalonJNI.intp_value(valuep)==0) ? false : true;
   }
-  // Returns temperature of Talon, in degrees Celsius.
+  /**
+   *  Returns temperature of Talon, in degrees Celsius.
+   */
   public double getTemp() {
     long tempp = CanTalonJNI.new_doublep(); // Create a new swig pointer.
     m_impl.GetTemp(new SWIGTYPE_p_double(tempp, true));
     return CanTalonJNI.doublep_value(tempp);
   }
 
-  // Returns the current going through the Talon, in Amperes.
+  /**
+   *  Returns the current going through the Talon, in Amperes.
+   */
   public double getOutputCurrent() {
     long curp = CanTalonJNI.new_doublep(); // Create a new swig pointer.
     m_impl.GetCurrent(new SWIGTYPE_p_double(curp, true));
@@ -398,6 +402,14 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
     return CanTalonJNI.doublep_value(voltagep);
   }
 
+  /**
+ * TODO documentation (see CANJaguar.java)
+ *
+ * @return The position of the sensor currently providing feedback.
+ * 			When using analog sensors, 0 units corresponds to 0V, 1023 units corresponds to 3.3V
+ * 			When using an analog encoder (wrapping around 1023 to 0 is possible) the units are still 3.3V per 1023 units.
+ * 			When using quadrature, each unit is a quadrature edge (4X) mode.
+ */
   public double getPosition() {
     long positionp = CanTalonJNI.new_intp();
     m_impl.GetSensorPosition(new SWIGTYPE_p_int(positionp, true));
@@ -408,6 +420,21 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
     m_impl.SetSensorPosition((int)pos);
   }
 
+/**
+ * TODO documentation (see CANJaguar.java)
+ *
+ * @return The speed of the sensor currently providing feedback.
+ *
+ * The speed units will be in the sensor's native ticks per 100ms.
+ *
+ * For analog sensors, 3.3V corresponds to 1023 units.
+ * 		So a speed of 200 equates to ~0.645 dV per 100ms or 6.451 dV per second.
+ * 		If this is an analog encoder, that likely means 1.9548 rotations per sec.
+ * For quadrature encoders, each unit corresponds a quadrature edge (4X).
+ * 		So a 250 count encoder will produce 1000 edge events per rotation.
+ * 		An example speed of 200 would then equate to 20% of a rotation per 100ms,
+ * 		or 10 rotations per second.
+ */
   public double getSpeed() {
     long speedp = CanTalonJNI.new_intp();
     m_impl.GetSensorVelocity(new SWIGTYPE_p_int(speedp, true));
@@ -462,7 +489,6 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
    * Get the current proportional constant.
    *
    * @return double proportional constant for current profile.
-   * @throws IllegalStateException if not in Position of Speed mode.
    */
   public double getP() {
 		//if(!(m_controlMode.equals(ControlMode.Position) || m_controlMode.equals(ControlMode.Speed))) {

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CameraServer.java

@@ -1,6 +1,7 @@
 package edu.wpi.first.wpilibj;
 
 import java.io.DataInputStream;
+import java.io.DataOutputStream;
 import java.io.IOException;
 import java.io.OutputStream;
 import java.net.InetSocketAddress;
@@ -8,8 +9,11 @@ import java.net.ServerSocket;
 import java.net.Socket;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
+import java.util.ArrayDeque;
 import java.util.ArrayList;
+import java.util.Deque;
 import java.util.List;
+import java.util.NoSuchElementException;
 import java.util.concurrent.atomic.AtomicBoolean;
 
 import com.ni.vision.NIVision;
@@ -17,262 +21,343 @@ import com.ni.vision.NIVision.Image;
 import com.ni.vision.NIVision.RawData;
 import com.ni.vision.VisionException;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.vision.USBCamera;
+
 //replicates CameraServer.cpp in java lib
 
 public class CameraServer {
 
-	public static CameraServer getInstance() {
-		if (server == null) {
-			server = new CameraServer();
-		}
-		return server;
-	}
+    private static final int kPort = 1180;
+    private static final byte[] kMagicNumber = { 0x01, 0x00, 0x00, 0x00 };
+    private static final int kSize640x480 = 0;
+    private static final int kSize320x240 = 1;
+    private static final int kSize160x120 = 2;
+    private static final int kHardwareCompression = -1;
+    private static final String kDefaultCameraName = "cam1";
+    private static final int kMaxImageSize = 200000;
+    private static CameraServer server;
 
-	private static CameraServer server;
-	private AtomicBoolean ready;
-	private Thread serverThread;
-	private int m_quality;
-	private static final int kPort = 1180;
-	private static final byte[] kMagicNumber = { 0x01, 0x00, 0x00, 0x00 };
-	private static final int kSize640x480 = 0;
-	private static final int kSize320x240 = 1;
-	private static final int kSize160x120 = 2;
-	private static final int kHardwareCompression = -1;
-	private static final String kDefaultCameraName = "cam1";
-	private List<Byte> m_imageData;
-	private boolean m_autoCaptureStarted;
+    public static CameraServer getInstance() {
+        if (server == null) {
+            server = new CameraServer();
+        }
+        return server;
+    }
 
-	private CameraServer() {
-		ready = new AtomicBoolean(false);
-		m_imageData = new ArrayList<Byte>();
-		m_quality = 50;
-		serverThread = new Thread(new Runnable() {
-			public void run() {
-				try {
-					serve();
-				} catch (IOException e) {
-					// do stuff here
-				} catch (InterruptedException e) {
-					// do stuff here
-				}
-			}
-		});
-		serverThread.start();
-	}
+    private Thread serverThread;
+    private int m_quality;
+    private boolean m_autoCaptureStarted;
+    private boolean m_hwClient = true;
+    private USBCamera m_camera;
+    private CameraData m_imageData;
+    private Deque<ByteBuffer> m_imageDataPool;
 
-	/**
-	 * 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 synchronized void setImage(Image image) {
-		// handle multi-threadedness
+    private class CameraData {
+        RawData data;
+        int start;
+        public CameraData(RawData d, int s) {
+            data = d;
+            start = s;
+        }
+    }
 
-		/* Flatten the IMAQ image to a JPEG */
+    private CameraServer() {
+        m_quality = 50;
+        m_camera = null;
+        m_imageData = null;
+        m_imageDataPool = new ArrayDeque<>(3);
+        for (int i = 0; i < 3; i++) {
+            m_imageDataPool.addLast(ByteBuffer.allocateDirect(kMaxImageSize));
+        }
+        serverThread = new Thread(new Runnable() {
+                public void run() {
+                    try {
+                        serve();
+                    } catch (IOException e) {
+                        // do stuff here
+                    } catch (InterruptedException e) {
+                        // do stuff here
+                    }
+                }
+            });
+        serverThread.setName("CameraServer Send Thread");
+        serverThread.start();
+    }
 
-		RawData data = NIVision.imaqFlatten(image,
-				NIVision.FlattenType.FLATTEN_IMAGE,
-				NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality);
-		ByteBuffer buffer = data.getBuffer();
-		m_imageData.clear();
+    private synchronized void setImageData(RawData data, int start) {
+        if (m_imageData != null && m_imageData.data != null) {
+            m_imageData.data.free();
+            if (m_imageData.data.getBuffer() != null)
+                m_imageDataPool.addLast(m_imageData.data.getBuffer());
+            m_imageData = null;
+        }
+        m_imageData = new CameraData(data, start);
+        notifyAll();
+    }
 
-		/* Find the start of the JPEG data */
-		int index = 0;
-		while (index < buffer.limit() - 1) {
-			if ((buffer.get(index) & 0xff) == 0xFF
-					&& (buffer.get(index + 1) & 0xff) == 0xD8)
-				break;
-			index++;
-		}
-		while (index < buffer.limit()) {
-			m_imageData.add(buffer.get(index++));
-		}
+    /**
+     * 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
 
-		if (m_imageData.size() <= 2) {
-			throw new VisionException(
-					"data size of flattened image is less than 2. Try another camera! ");
-		}
+        /* Flatten the IMAQ image to a JPEG */
 
-		data.free();
+        RawData data = NIVision.imaqFlatten(image,
+                                            NIVision.FlattenType.FLATTEN_IMAGE,
+                                            NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality);
+        ByteBuffer buffer = data.getBuffer();
+        boolean hwClient;
 
-		ready.set(true);
-	}
+        synchronized (this) {
+            hwClient = m_hwClient;
+        }
 
-	/**
-	 * 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) {
-		synchronized (this) {
-			if (m_autoCaptureStarted) {
-				// print
-				// "you fucked up \"Automatic capture has already been started\""
-				return;
-			}
-			m_autoCaptureStarted = true;
-		}
+        /* 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++;
+            }
+        }
 
-		CaptureRunnable runnable = new CaptureRunnable(cameraName);
-		Thread captureThread = new Thread(runnable);
-		captureThread.start();
+        if (buffer.limit() - index - 1 <= 2) {
+            throw new VisionException("data size of flattened image is less than 2. Try another camera! ");
+        }
 
-	}
+        setImageData(data, index);
+    }
 
-	private class CaptureRunnable implements Runnable {
-		String name;
+    /**
+     * 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.
+     * This overload calles {@link #startAutomaticCapture(String)} with the
+     * default camera name
+     */
+    public void startAutomaticCapture() {
+        startAutomaticCapture(USBCamera.kDefaultCameraName);
+    }
 
-		public CaptureRunnable(String name) {
-			this.name = name;
-		}
+    /**
+     * 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) {
+        USBCamera camera = new USBCamera(cameraName);
+        camera.openCamera();
+        startAutomaticCapture(camera);
+    }
 
-		@Override
-		public void run() {
-			Image frame = NIVision.imaqCreateImage(
-					NIVision.ImageType.IMAGE_RGB, 0);
-			int id = NIVision.IMAQdxOpenCamera(name,
-				NIVision.IMAQdxCameraControlMode.CameraControlModeController);
-			NIVision.IMAQdxConfigureGrab(id);
-			NIVision.IMAQdxStartAcquisition(id);
+    public synchronized void startAutomaticCapture(USBCamera camera) {
+        if (m_autoCaptureStarted) return;
+        m_autoCaptureStarted = true;
+        m_camera = camera;
 
-			while (true) {
-				NIVision.IMAQdxGrab(id, frame, 1);
-				setImage(frame);
-			}
+        m_camera.startCapture();
 
-			// NIVision.IMAQdxStopAcquisition(id);
-			// NIVision.IMAQdxCloseCamera(id);
+        Thread captureThread = new Thread(new Runnable() {
+                @Override
+                public void run() {
+                    capture();
+                }
+            });
+        captureThread.setName("Camera Capture Thread");
+        captureThread.start();
+    }
 
-		}
+    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);
+                    }
+                }
+            }
+        }
+    }
 
-	/**
-	 * check if auto capture is started
-	 *
-	 */
-	public synchronized boolean isAutoCaptureStarted() {
-		return m_autoCaptureStarted;
-	}
 
-	/**
-	 * Set the quality of the compressed image sent to the dashboard
-	 *
-	 * @param quality
-	 *            The quality of the JPEG image, from 0 to 100
-	 */
-	public synchronized void setQuality(int quality) {
-		m_quality = quality > 100 ? 100 : quality < 0 ? 0 : quality;
-	}
 
-	/**
-	 * Get the quality of the compressed image sent to the dashboard
-	 *
-	 * @return The quality, from 0 to 100
-	 */
-	public synchronized int getQuality() {
-		return m_quality;
-	}
+    /**
+     * check if auto capture is started
+     *
+     */
+    public synchronized boolean isAutoCaptureStarted() {
+        return m_autoCaptureStarted;
+    }
 
-	/**
-	 * Run the M-JPEG server.
-	 *
-	 * This function listens for a connection from the dashboard in a background
-	 * thread, then sends back the M-JPEG stream.
-	 * 
-	 * @throws IOException if the Socket connection fails
-	 * @throws InterruptedException if the sleep is interrupted
-	 */
-	protected void serve() throws IOException, InterruptedException {
+    /**
+     * Sets the size of the image to use. Use the public kSize constants
+     * to set the correct mode, or set it directory on a camera and call
+     * the appropriate autoCapture method
+     * @param size The size to use
+     */
+    public synchronized void setSize(int size) {
+        if (m_camera == null) return;
+        switch (size) {
+        case kSize640x480:
+            m_camera.setSize(640, 480);
+            break;
+        case kSize320x240:
+            m_camera.setSize(320, 240);
+            break;
+        case kSize160x120:
+            m_camera.setSize(160, 120);
+            break;
+        }
+    }
 
-		ServerSocket socket = new ServerSocket();
-		socket.setReuseAddress(true);
-		InetSocketAddress address = new InetSocketAddress(kPort);
-		socket.bind(address);
+    /**
+     * Set the quality of the compressed image sent to the dashboard
+     *
+     * @param quality
+     *            The quality of the JPEG image, from 0 to 100
+     */
+    public synchronized void setQuality(int quality) {
+        m_quality = quality > 100 ? 100 : quality < 0 ? 0 : quality;
+    }
 
-		while (true) {
-			try {
-				Socket s = socket.accept();
+    /**
+     * Get the quality of the compressed image sent to the dashboard
+     *
+     * @return The quality, from 0 to 100
+     */
+    public synchronized int getQuality() {
+        return m_quality;
+    }
 
-				DataInputStream is = new DataInputStream(s.getInputStream());
-				OutputStream os = s.getOutputStream();
+    /**
+     * Run the M-JPEG server.
+     *
+     * This function listens for a connection from the dashboard in a background
+     * thread, then sends back the M-JPEG stream.
+     *
+     * @throws IOException if the Socket connection fails
+     * @throws InterruptedException if the sleep is interrupted
+     */
+    protected void serve() throws IOException, InterruptedException {
 
-				int fps = is.readInt();
-				int compression = is.readInt();
-				int size = is.readInt();
+        ServerSocket socket = new ServerSocket();
+        socket.setReuseAddress(true);
+        InetSocketAddress address = new InetSocketAddress(kPort);
+        socket.bind(address);
 
-				if (compression != kHardwareCompression) {
-					// print to driverstation -->
-					// ("Choose \"USB Camera HW\" on the dashboard");
-					s.close();
-					continue;
-				}
+        while (true) {
+            try {
+                Socket s = socket.accept();
 
-				long period = (long) (1000 / (1.0 * fps));
-				while (true) {
+                DataInputStream is = new DataInputStream(s.getInputStream());
+                DataOutputStream os = new DataOutputStream(s.getOutputStream());
 
-					long t0 = System.currentTimeMillis();
+                int fps = is.readInt();
+                int compression = is.readInt();
+                int size = is.readInt();
 
-					while (!ready.get())
-						;
-					ready.set(false);
+                if (compression != kHardwareCompression) {
+                    DriverStation.reportError("Choose \"USB Camera HW\" on the dashboard", false);
+                    s.close();
+                    continue;
+                }
 
-					// write numbers
-					try {
+                // Wait for the camera
+                synchronized (this) {
+                    System.out.println("Camera not yet ready, awaiting image");
+                    if (m_camera == null) wait();
+                    m_hwClient = compression == kHardwareCompression;
+                    if (!m_hwClient) setQuality(100 - compression);
+                    else if (m_camera != null) m_camera.setFPS(fps);
+                    setSize(size);
+                }
 
-						os.write(kMagicNumber);
+                long period = (long) (1000 / (1.0 * fps));
+                while (true) {
+                    long t0 = System.currentTimeMillis();
+                    CameraData imageData = null;
+                    synchronized (this) {
+                        wait();
+                        imageData = m_imageData;
+                        m_imageData = null;
+                    }
 
-						// write size of image
+                    if (imageData == null) continue;
+                    // Set the buffer position to the start of the data,
+                    // and then create a new wrapper for the data at
+                    // exactly that position
+                    imageData.data.getBuffer().position(imageData.start);
+                    byte[] imageArray = new byte[imageData.data.getBuffer().remaining()];
+                    imageData.data.getBuffer().get(imageArray, 0, imageData.data.getBuffer().remaining());
 
-						synchronized (this) {
-							os.write(intTobyteArray(m_imageData.size()));
+                    // write numbers
+                    try {
+                        os.write(kMagicNumber);
+                        os.writeInt(imageArray.length);
+                        os.write(imageArray);
+                        os.flush();
+                        long dt = System.currentTimeMillis() - t0;
 
-							byte[] imageData = new byte[m_imageData.size()];
-
-							for (int i = 0; i < m_imageData.size(); i++) {
-								imageData[i] = m_imageData.get(i).byteValue();
-							}
-
-							os.write(imageData);
-						}
-						long dt = System.currentTimeMillis() - t0;
-
-						if (dt < period) {
-							Thread.sleep(period - dt);
-						}
-
-					} catch (IOException ex) {
-						// print error to driverstation
-						break;
-					}
-				}
-
-			} catch (IOException ex) {
-				// print error to driverstation
-				continue;
-			}
-		}
-	}
-
-	public byte[] intTobyteArray(int n) {
-		byte[] arr = new byte[4];
-
-		arr[0] = (byte) ((n >> 24) & 0xFF);
-		arr[1] = (byte) ((n >> 16) & 0xFF);
-		arr[2] = (byte) ((n >> 8) & 0xFF);
-		arr[3] = (byte) (n & 0xFF);
-
-		return arr;
-	}
+                        if (dt < period) {
+                            Thread.sleep(period - dt);
+                        }
+                    } catch (IOException | UnsupportedOperationException ex) {
+                        DriverStation.reportError(ex.getMessage(), true);
+                        break;
+                    } finally {
+                        imageData.data.free();
+                        if (imageData.data.getBuffer() != null) {
+                            synchronized (this) {
+                                m_imageDataPool.addLast(imageData.data.getBuffer());
+                            }
+                        }
+                    }
+                }
+            } catch (IOException ex) {
+                DriverStation.reportError(ex.getMessage(), true);
+                continue;
+            }
+        }
+    }
 }

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Counter.java

@@ -196,7 +196,7 @@ public class Counter extends SensorBase implements CounterBase,
 			throw new NullPointerException("The Analog Trigger given was null");
 		}
 		initCounter(Mode.kTwoPulse);
-		setUpSource(trigger.createOutput(AnalogTriggerType.STATE));
+		setUpSource(trigger.createOutput(AnalogTriggerType.kState));
 	}
 
 	@Override

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java

@@ -76,8 +76,8 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable {
     /**
      * Constructor.
      * Uses the default PCM ID of 0
-     * @param forwardChannel The forward channel number on the PCM.
-     * @param reverseChannel The reverse channel number on the PCM.
+     * @param forwardChannel The forward channel number on the PCM (0..7).
+     * @param reverseChannel The reverse channel number on the PCM (0..7).
      */
     public DoubleSolenoid(final int forwardChannel, final int reverseChannel) {
         super(getDefaultSolenoidModule());
@@ -90,8 +90,8 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable {
      * Constructor.
      *
      * @param moduleNumber The module number of the solenoid module to use.
-     * @param forwardChannel The forward channel on the module to control.
-     * @param reverseChannel The reverse channel on the module to control.
+     * @param forwardChannel The forward channel on the module to control (0..7).
+     * @param reverseChannel The reverse channel on the module to control (0..7).
      */
     public DoubleSolenoid(final int moduleNumber, final int forwardChannel, final int reverseChannel) {
         super(moduleNumber);

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -267,7 +267,7 @@ public class DriverStation implements RobotState.Interface {
 
         if (pov >= m_joystickPOVs[stick].length) {
 			reportJoystickUnpluggedError("WARNING: Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in\n");
-            return 0;
+            return -1;
         }
 
         return m_joystickPOVs[stick][pov];

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Encoder.java

@@ -33,6 +33,9 @@ import edu.wpi.first.wpilibj.util.BoundaryException;
  * to be zeroed before use.
  */
 public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveWindowSendable {
+	public enum IndexingType {
+		kResetWhileHigh, kResetWhileLow, kResetOnFallingEdge, kResetOnRisingEdge
+	}
 
 	/**
 	 * The a source
@@ -213,6 +216,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
 		m_bSource = new DigitalInput(bChannel);
 		m_indexSource = new DigitalInput(indexChannel);
 		initEncoder(reverseDirection);
+		setIndexSource(indexChannel);
 	}
 
 	/**
@@ -325,7 +329,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
 	}
 
 	/**
-	 * Encoder constructor. Construct a Encoder given a and b channels as
+	 * Encoder constructor. Construct a Encoder given a, b and index channels as
 	 * digital inputs. This is used in the case where the digital inputs are
 	 * shared. The Encoder class will not allocate the digital inputs and assume
 	 * that they already are counted.
@@ -357,10 +361,11 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
 		m_bSource = bSource;
 		m_indexSource = indexSource;
 		initEncoder(reverseDirection);
+		setIndexSource(indexSource);
 	}
 
 	/**
-	 * Encoder constructor. Construct a Encoder given a and b channels as
+	 * Encoder constructor. Construct a Encoder given a, b and index channels as
 	 * digital inputs. This is used in the case where the digital inputs are
 	 * shared. The Encoder class will not allocate the digital inputs and assume
 	 * that they already are counted.
@@ -727,6 +732,59 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
 		}
 	}
 
+	/**
+	 * Set the index source for the encoder.  When this source rises, the encoder count automatically resets.
+	 *
+	 * @param channel A DIO channel to set as the encoder index
+	 * @param type The state that will cause the encoder to reset
+	 */
+	public void setIndexSource(int channel, IndexingType type) {
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
+			
+		boolean activeHigh = (type == IndexingType.kResetWhileHigh) || (type == IndexingType.kResetOnRisingEdge);
+		boolean edgeSensitive = (type == IndexingType.kResetOnFallingEdge) || (type == IndexingType.kResetOnRisingEdge);
+
+		EncoderJNI.setEncoderIndexSource(m_encoder, channel, false, activeHigh, edgeSensitive, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
+	}
+
+	/**
+	 * Set the index source for the encoder.  When this source is activated, the encoder count automatically resets.
+	 *
+	 * @param channel A DIO channel to set as the encoder index
+	 */
+	public void setIndexSource(int channel) {
+		this.setIndexSource(channel, IndexingType.kResetOnRisingEdge);
+	}
+
+	/**
+	 * Set the index source for the encoder.  When this source rises, the encoder count automatically resets.
+	 *
+	 * @param source A digital source to set as the encoder index
+	 * @param type The state that will cause the encoder to reset
+	 */
+	public void setIndexSource(DigitalSource source, IndexingType type) {
+		ByteBuffer status = ByteBuffer.allocateDirect(4);
+		status.order(ByteOrder.LITTLE_ENDIAN);
+			
+		boolean activeHigh = (type == IndexingType.kResetWhileHigh) || (type == IndexingType.kResetOnRisingEdge);
+		boolean edgeSensitive = (type == IndexingType.kResetOnFallingEdge) || (type == IndexingType.kResetOnRisingEdge);
+
+		EncoderJNI.setEncoderIndexSource(m_encoder, source.getChannelForRouting(), source.getAnalogTriggerForRouting(),
+				activeHigh, edgeSensitive, status.asIntBuffer());
+		HALUtil.checkStatus(status.asIntBuffer());
+	}
+
+	/**
+	 * Set the index source for the encoder.  When this source is activated, the encoder count automatically resets.
+	 *
+	 * @param source A digital source to set as the encoder index
+	 */
+	public void setIndexSource(DigitalSource source) {
+		this.setIndexSource(source, IndexingType.kResetOnRisingEdge);
+	}
+
 	/*
 	 * Live Window code, only does anything if live window is activated.
 	 */

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java

@@ -17,7 +17,8 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.tables.ITable;
 
 /**
- * Class for getting voltage, current, and temperature from the CAN PDP
+ * Class for getting voltage, current, temperature, power and energy from the CAN PDP.
+ * The PDP must be at CAN Address 0.
  * @author Thomas Clark
  */
 public class PowerDistributionPanel extends SensorBase implements LiveWindowSendable {
@@ -26,7 +27,7 @@ public class PowerDistributionPanel extends SensorBase implements LiveWindowSend
 
 	/**
 	 * Query the input voltage of the PDP
-	 * @return The voltage of the PDP
+	 * @return The voltage of the PDP in volts
 	 */
 	public double getVoltage() {
 		ByteBuffer status = ByteBuffer.allocateDirect(4);
@@ -67,7 +68,7 @@ public class PowerDistributionPanel extends SensorBase implements LiveWindowSend
 
 	/**
 	* Query the current of all monitored PDP channels (0-15)
-	* @return The current of all the channels
+	* @return The current of all the channels in Amperes
 	*/
 	public double getTotalCurrent(){
 		ByteBuffer status = ByteBuffer.allocateDirect(4);
@@ -80,7 +81,7 @@ public class PowerDistributionPanel extends SensorBase implements LiveWindowSend
 
 	/**
 	* Query the total power drawn from the monitored PDP channels
-	* @return the total power
+	* @return the total power in Watts
 	*/
 	public double getTotalPower(){
 		ByteBuffer status = ByteBuffer.allocateDirect(4);
@@ -94,7 +95,7 @@ public class PowerDistributionPanel extends SensorBase implements LiveWindowSend
 
 	/**
 	* Query the total energy drawn from the monitored PDP channels
-	* @return the total power
+	* @return the total energy in Joules
 	*/
 	public double getTotalEnergy(){
 		ByteBuffer status = ByteBuffer.allocateDirect(4);

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Relay.java

@@ -152,7 +152,7 @@ public class Relay extends SensorBase implements LiveWindowSendable {
 	 * Relay constructor given a channel.
 	 *
 	 * @param channel
-	 *            The channel number for this relay.
+	 *            The channel number for this relay (0 - 3).
 	 * @param direction
 	 *            The direction that the Relay object will control.
 	 */
@@ -169,7 +169,7 @@ public class Relay extends SensorBase implements LiveWindowSendable {
 	 * Relay constructor given a channel, allowing both directions.
 	 *
 	 * @param channel
-	 *            The channel number for this relay.
+	 *            The channel number for this relay (0 - 3).
 	 */
 	public Relay(final int channel) {
 		this(channel, Direction.kBoth);

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -216,7 +216,7 @@ public abstract class RobotBase {
 
                 output = new FileOutputStream(file);
 
-				output.write("2015 Java 1.0.0".getBytes());
+				output.write("2015 Java 1.2.0".getBytes());
 
             } catch (IOException ex) {
                 ex.printStackTrace();

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java

@@ -6,14 +6,13 @@
 /*----------------------------------------------------------------------------*/
 package edu.wpi.first.wpilibj;
 
-import edu.wpi.first.wpilibj.can.CANNotInitializedException;
 import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances;
 import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
 import edu.wpi.first.wpilibj.communication.UsageReporting;
 
 /**
  * Utility class for handling Robot drive based on a definition of the motor configuration.
- * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and 
+ * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and
  * mecanum drive trains are supported. In the future other drive types like swerve might
  * be implemented. Motor channel numbers are supplied on creation of the class. Those are
  * used for either the drive function (intended for hand created drive code, such as autonomous)
@@ -69,7 +68,7 @@ public class RobotDrive implements MotorSafety {
     protected SpeedController m_rearLeftMotor;
     protected SpeedController m_rearRightMotor;
     protected boolean m_allocatedSpeedControllers;
-    protected boolean m_isCANInitialized = false;//TODO: fix can
+    protected byte m_syncGroup = 0;
     protected static boolean kArcadeRatioCurve_Reported = false;
     protected static boolean kTank_Reported = false;
     protected static boolean kArcadeStandard_Reported = false;
@@ -488,20 +487,13 @@ public class RobotDrive implements MotorSafety {
         wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;
 
         normalize(wheelSpeeds);
+        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup);
+        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup);
+        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup);
+        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup);
 
-        byte syncGroup = (byte)0x80;
-
-        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
-        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
-        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
-        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
-
-        if (m_isCANInitialized) {
-            try {
-                CANJaguar.updateSyncGroup(syncGroup);
-            } catch (CANNotInitializedException e) {
-                m_isCANInitialized = false;
-            }
+        if (m_syncGroup != 0) {
+            CANJaguar.updateSyncGroup(m_syncGroup);
         }
 
         if (m_safetyHelper != null) m_safetyHelper.feed();
@@ -541,19 +533,13 @@ public class RobotDrive implements MotorSafety {
 
         normalize(wheelSpeeds);
 
-        byte syncGroup = (byte)0x80;
+        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup);
+        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup);
+        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup);
+        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup);
 
-        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
-        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
-        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
-        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
-
-        if (m_isCANInitialized) {
-            try {
-                CANJaguar.updateSyncGroup(syncGroup);
-            } catch (CANNotInitializedException e) {
-                m_isCANInitialized = false;
-            }
+        if (this.m_syncGroup != 0) {
+            CANJaguar.updateSyncGroup(m_syncGroup);
         }
 
         if (m_safetyHelper != null) m_safetyHelper.feed();
@@ -586,24 +572,18 @@ public class RobotDrive implements MotorSafety {
             throw new NullPointerException("Null motor provided");
         }
 
-        byte syncGroup = (byte)0x80;
-
         if (m_frontLeftMotor != null) {
-            m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
+            m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup);
         }
-        m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
+        m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup);
 
         if (m_frontRightMotor != null) {
-            m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
+            m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup);
         }
-        m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
+        m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup);
 
-        if (m_isCANInitialized) {
-            try {
-                CANJaguar.updateSyncGroup(syncGroup);
-            } catch (CANNotInitializedException e) {
-                m_isCANInitialized = false;
-            }
+        if (this.m_syncGroup != 0) {
+            CANJaguar.updateSyncGroup(m_syncGroup);
         }
 
         if (m_safetyHelper != null) m_safetyHelper.feed();
@@ -681,6 +661,15 @@ public class RobotDrive implements MotorSafety {
         m_maxOutput = maxOutput;
     }
 
+    /**
+     * Set the number of the sync group for the motor controllers.  If the motor controllers are {@link CANJaguar}s,
+     * then they will all be added to this sync group, causing them to update their values at the same time.
+     *
+     * @param syncGroup the update group to add the motor controllers to
+     */
+    public void setCANJaguarSyncGroup(byte syncGroup) {
+        m_syncGroup = syncGroup;
+    }
 
     /**
      * Free the speed controllers if they were allocated locally

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SerialPort.java

@@ -362,9 +362,9 @@ public class SerialPort {
 	    ByteBuffer status = ByteBuffer.allocateDirect(4);
 		status.order(ByteOrder.LITTLE_ENDIAN);
 		ByteBuffer dataReceivedBuffer = ByteBuffer.allocateDirect(count);
-        SerialPortJNI.serialRead(m_port, dataReceivedBuffer, count, status.asIntBuffer());
+        int gotten = SerialPortJNI.serialRead(m_port, dataReceivedBuffer, count, status.asIntBuffer());
 		HALUtil.checkStatus(status.asIntBuffer());
-		byte[] retVal = new byte[count];
+		byte[] retVal = new byte[gotten];
 		dataReceivedBuffer.get(retVal);
 		return retVal;
     }

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Solenoid.java

@@ -53,7 +53,7 @@ public class Solenoid extends SolenoidBase implements LiveWindowSendable {
     /**
      * Constructor using the default PCM ID (0)
      *
-     * @param channel The channel on the PCM to control.
+     * @param channel The channel on the PCM to control (0..7).
      */
     public Solenoid(final int channel) {
         super(getDefaultSolenoidModule());

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/EncoderJNI.java

@@ -15,4 +15,5 @@ public class EncoderJNI extends JNIWrapper {
 	public static native void setEncoderReverseDirection(ByteBuffer encoder_pointer, byte reverseDirection, IntBuffer status);
 	public static native void setEncoderSamplesToAverage(ByteBuffer encoder_pointer, int samplesToAverage, IntBuffer status);
 	public static native int getEncoderSamplesToAverage(ByteBuffer encoder_pointer, IntBuffer status);
+	public static native void setEncoderIndexSource(ByteBuffer digital_port, int pin, boolean analogTrigger, boolean activeHigh, boolean edgeSensitive, IntBuffer status);
 }

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/internal/HardwareTimer.java

@@ -133,7 +133,7 @@ public class HardwareTimer implements Timer.StaticInterface {
             if (get() > period) {
                 // Advance the start time by the period.
                 // Don't set it to the current time... we want to avoid drift.
-                m_startTime += period;
+                m_startTime += period * 1000;
                 return true;
             }
             return false;

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/vision/USBCamera.java

@@ -0,0 +1,300 @@
+package edu.wpi.first.wpilibj.vision;
+
+import java.nio.ByteBuffer;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+
+import com.ni.vision.NIVision;
+import com.ni.vision.VisionException;
+
+import static com.ni.vision.NIVision.Image;
+import static edu.wpi.first.wpilibj.Timer.delay;
+
+public class USBCamera {
+	public static String kDefaultCameraName = "cam0";
+
+	private static String ATTR_VIDEO_MODE = "AcquisitionAttributes::VideoMode";
+	private static String ATTR_WB_MODE = "CameraAttributes::WhiteBalance::Mode";
+	private static String ATTR_WB_VALUE = "CameraAttributes::WhiteBalance::Value";
+	private static String ATTR_EX_MODE = "CameraAttributes::Exposure::Mode";
+	private static String ATTR_EX_VALUE = "CameraAttributes::Exposure::Value";
+	private static String ATTR_BR_MODE = "CameraAttributes::Brightness::Mode";
+	private static String ATTR_BR_VALUE = "CameraAttributes::Brightness::Value";
+
+	public class WhiteBalance {
+		public static final int kFixedIndoor = 3000;
+		public static final int kFixedOutdoor1 = 4000;
+		public static final int kFixedOutdoor2 = 5000;
+		public static final int kFixedFluorescent1 = 5100;
+		public static final int kFixedFlourescent2 = 5200;
+	}
+
+	private Pattern m_reMode = Pattern.compile("(?<width>[0-9]+)\\s*x\\s*(?<height>[0-9]+)\\s+(?<format>.*?)\\s+(?<fps>[0-9.]+)\\s*fps");
+
+	private String m_name = kDefaultCameraName;
+	private int m_id = -1;
+	private boolean m_active = false;
+	private boolean m_useJpeg = true;
+	private int m_width = 320;
+	private int m_height = 240;
+	private int m_fps = 30;
+	private String m_whiteBalance = "auto";
+	private int m_whiteBalanceValue = -1;
+	private String m_exposure = "auto";
+	private int m_exposureValue = -1;
+	private int m_brightness = 50;
+	private boolean m_needSettingsUpdate = true;
+
+	public USBCamera() {
+		openCamera();
+	}
+
+	public USBCamera(String name) {
+		m_name = name;
+		openCamera();
+	}
+
+	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;
+		}
+	}
+
+	public synchronized void closeCamera() {
+		if (m_id == -1)
+			return;
+		NIVision.IMAQdxCloseCamera(m_id);
+		m_id = -1;
+	}
+
+	public synchronized void startCapture() {
+		if (m_id == -1 || m_active)
+			return;
+		NIVision.IMAQdxConfigureGrab(m_id);
+		NIVision.IMAQdxStartAcquisition(m_id);
+		m_active = true;
+	}
+
+	public synchronized void stopCapture() {
+		if (m_id == -1 || !m_active)
+			return;
+		NIVision.IMAQdxStopAcquisition(m_id);
+		NIVision.IMAQdxUnconfigureAcquisition(m_id);
+		m_active = false;
+	}
+
+	public synchronized void updateSettings() {
+		boolean wasActive = m_active;
+		// Stop acquistion, close and reopen camera
+		if (wasActive)
+			stopCapture();
+		if (m_id != -1)
+			closeCamera();
+		openCamera();
+
+		// Video Mode
+		NIVision.dxEnumerateVideoModesResult enumerated = NIVision.IMAQdxEnumerateVideoModes(m_id);
+		NIVision.IMAQdxEnumItem foundMode = null;
+		int foundFps = 1000;
+		for (NIVision.IMAQdxEnumItem mode : enumerated.videoModeArray) {
+			Matcher m = m_reMode.matcher(mode.Name);
+			if (!m.matches())
+				continue;
+			if (Integer.parseInt(m.group("width")) != m_width)
+				continue;
+			if (Integer.parseInt(m.group("height")) != m_height)
+				continue;
+			double fps = Double.parseDouble(m.group("fps"));
+			if (fps < m_fps)
+				continue;
+			if (fps > foundFps)
+				continue;
+			String format = m.group("format");
+			boolean isJpeg = format.equals("jpeg") || format.equals("JPEG");
+			if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg))
+				continue;
+			foundMode = mode;
+			foundFps = (int)fps;
+		}
+		if (foundMode != null) {
+			System.out.println("found mode " + foundMode.Value + ": " + foundMode.Name);
+			if (foundMode.Value != enumerated.currentMode)
+				NIVision.IMAQdxSetAttributeU32(m_id, ATTR_VIDEO_MODE, foundMode.Value);
+		}
+
+		// White Balance
+		if (m_whiteBalance == "auto")
+			NIVision.IMAQdxSetAttributeString(m_id, ATTR_WB_MODE, "Auto");
+		else {
+			NIVision.IMAQdxSetAttributeString(m_id, ATTR_WB_MODE, "Manual");
+			if (m_whiteBalanceValue != -1)
+				NIVision.IMAQdxSetAttributeI64(m_id, ATTR_WB_VALUE, m_whiteBalanceValue);
+		}
+
+		// Exposure
+		if (m_exposure == "auto")
+			NIVision.IMAQdxSetAttributeString(m_id, ATTR_EX_MODE, "AutoAperaturePriority");
+		else {
+			NIVision.IMAQdxSetAttributeString(m_id, ATTR_EX_MODE, "Manual");
+			if (m_exposureValue != -1) {
+				long minv = NIVision.IMAQdxGetAttributeMinimumI64(m_id, ATTR_EX_VALUE);
+				long maxv = NIVision.IMAQdxGetAttributeMaximumI64(m_id, ATTR_EX_VALUE);
+				long val = minv + (long)(((double)(maxv - minv)) * (((double)m_exposureValue) / 100.0));
+				NIVision.IMAQdxSetAttributeI64(m_id, ATTR_EX_VALUE, val);
+			}
+		}
+
+		// Brightness
+		NIVision.IMAQdxSetAttributeString(m_id, ATTR_BR_MODE, "Manual");
+		long minv = NIVision.IMAQdxGetAttributeMinimumI64(m_id, ATTR_BR_VALUE);
+		long maxv = NIVision.IMAQdxGetAttributeMaximumI64(m_id, ATTR_BR_VALUE);
+		long val = minv + (long)(((double)(maxv - minv)) * (((double)m_brightness) / 100.0));
+		NIVision.IMAQdxSetAttributeI64(m_id, ATTR_BR_VALUE, val);
+
+		// Restart acquisition
+		if (wasActive)
+			startCapture();
+	}
+
+	public synchronized void setFPS(int fps) {
+		if (fps != m_fps) {
+			m_needSettingsUpdate = true;
+			m_fps = fps;
+		}
+	}
+
+	public synchronized void setSize(int width, int height) {
+		if (width != m_width || height != m_height) {
+			m_needSettingsUpdate = true;
+			m_width = width;
+			m_height = height;
+		}
+	}
+
+	/** Set the brightness, as a percentage (0-100). */
+	public synchronized void setBrightness(int brightness) {
+		if (brightness > 100)
+			m_brightness = 100;
+		else if (brightness < 0)
+			m_brightness = 0;
+		else
+			m_brightness = brightness;
+		m_needSettingsUpdate = true;
+	}
+
+	/** Get the brightness, as a percentage (0-100). */
+	public synchronized int getBrightness() {
+		return m_brightness;
+	}
+
+	/** Set the white balance to auto. */
+	public synchronized void setWhiteBalanceAuto() {
+		m_whiteBalance = "auto";
+		m_whiteBalanceValue = -1;
+		m_needSettingsUpdate = true;
+	}
+
+	/** Set the white balance to hold current. */
+	public synchronized void setWhiteBalanceHoldCurrent() {
+		m_whiteBalance = "manual";
+		m_whiteBalanceValue = -1;
+		m_needSettingsUpdate = true;
+	}
+
+	/** Set the white balance to manual, with specified color temperature. */
+	public synchronized void setWhiteBalanceManual(int value) {
+		m_whiteBalance = "manual";
+		m_whiteBalanceValue = value;
+		m_needSettingsUpdate = true;
+	}
+
+	/** Set the exposure to auto aperature. */
+	public synchronized void setExposureAuto() {
+		m_exposure = "auto";
+		m_exposureValue = -1;
+		m_needSettingsUpdate = true;
+	}
+
+	/** Set the exposure to hold current. */
+	public synchronized void setExposureHoldCurrent() {
+		m_exposure = "manual";
+		m_exposureValue = -1;
+		m_needSettingsUpdate = true;
+	}
+
+	/** Set the exposure to manual, as a percentage (0-100). */
+	public synchronized void setExposureManual(int value) {
+		m_exposure = "manual";
+		if (value > 100)
+			m_exposureValue = 100;
+		else if (value < 0)
+			m_exposureValue = 0;
+		else
+			m_exposureValue = value;
+		m_needSettingsUpdate = true;
+	}
+
+	public synchronized void getImage(Image image) {
+		if (m_needSettingsUpdate || m_useJpeg) {
+			m_needSettingsUpdate = false;
+			m_useJpeg = false;
+			updateSettings();
+		}
+
+		NIVision.IMAQdxGrab(m_id, image, 1);
+	}
+
+	public synchronized void getImageData(ByteBuffer data) {
+		if (m_needSettingsUpdate || !m_useJpeg) {
+			m_needSettingsUpdate = false;
+			m_useJpeg = true;
+			updateSettings();
+		}
+
+		NIVision.IMAQdxGetImageData(m_id, data, NIVision.IMAQdxBufferNumberMode.BufferNumberModeLast, 0);
+                data.limit(data.capacity() - 1);
+		data.limit(getJpegSize(data));
+	}
+
+	private static int getJpegSize(ByteBuffer data) {
+		if (data.get(0) != (byte) 0xff || data.get(1) != (byte) 0xd8)
+			throw new VisionException("invalid image");
+		int pos = 2;
+		while (true) {
+			try {
+				byte b = data.get(pos);
+				if (b != (byte) 0xff)
+					throw new VisionException("invalid image at pos " + pos + " (" + data.get(pos) + ")");
+				b = data.get(pos+1);
+				if (b == (byte) 0x01 || (b >= (byte) 0xd0 && b <= (byte) 0xd7)) // various
+					pos += 2;
+				else if (b == (byte) 0xd9) // EOI
+					return pos + 2;
+				else if (b == (byte) 0xd8) // SOI
+					throw new VisionException("invalid image");
+				else if (b == (byte) 0xda) { // SOS
+					int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff);
+					pos += len + 2;
+					// Find next marker.  Skip over escaped and RST markers.
+					while (data.get(pos) != (byte) 0xff || data.get(pos+1) == (byte) 0x00 || (data.get(pos+1) >= (byte) 0xd0 && data.get(pos+1) <= (byte) 0xd7))
+						pos += 1;
+				} else { // various
+					int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff);
+					pos += len + 2;
+				}
+			} catch (IndexOutOfBoundsException ex) {
+				throw new VisionException("invalid image: could not find jpeg end " + ex.getMessage());
+			}
+		}
+	}
+}

wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogCrossConnectTest.java

@@ -165,7 +165,7 @@ public class AnalogCrossConnectTest extends AbstractInterruptTest {
 	InterruptableSensorBase giveInterruptableSensorBase() {
 		interruptTrigger = new AnalogTrigger(analogIO.getInput());
 		interruptTrigger.setLimitsVoltage(2.0f, 3.0f);
-		interruptTriggerOutput = new AnalogTriggerOutput(interruptTrigger, AnalogTriggerType.STATE);
+		interruptTriggerOutput = new AnalogTriggerOutput(interruptTrigger, AnalogTriggerType.kState);
 		return interruptTriggerOutput;
 	}
 

wpilibj/wpilibJavaJNI/lib/DIOJNI.cpp

@@ -113,10 +113,17 @@ JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIO
  * Signature: (Ljava/nio/ByteBuffer;Ljava/nio/IntBuffer;)B
  */
 JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIODirection
-  (JNIEnv *, jclass, jobject, jobject)
+  (JNIEnv *env, jclass, jobject id, jobject status)
 {
-	assert(false);
-
+	DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getDIODirection (RR upd)";
+	void ** javaId = (void**)env->GetDirectBufferAddress(id);
+	//DIOJNI_LOG(logDEBUG) << "Port Ptr = " << *javaId;
+	jint * statusPtr = (jint*)env->GetDirectBufferAddress(status);
+	//DIOJNI_LOG(logDEBUG) << "Status Ptr = " << statusPtr;
+	jbyte returnValue = getDIODirection(*javaId, statusPtr);
+	//DIOJNI_LOG(logDEBUG) << "Status = " << *statusPtr;
+	DIOJNI_LOG(logDEBUG) << "getDIODirectionResult = " << (jbyte)returnValue;
+	return returnValue;
 }
 
 /*
@@ -125,10 +132,16 @@ JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIODirection
  * Signature: (Ljava/nio/ByteBuffer;DLjava/nio/IntBuffer;)V
  */
 JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_pulse
-  (JNIEnv *, jclass, jobject, jdouble, jobject)
+  (JNIEnv *env, jclass, jobject id, jdouble value, jobject status)
 {
-	assert(false);
-
+	DIOJNI_LOG(logDEBUG) << "Calling DIOJNI pulse (RR upd)";
+	void ** javaId = (void**)env->GetDirectBufferAddress(id);
+	//DIOJNI_LOG(logDEBUG) << "Port Ptr = " << *javaId;
+	//DIOJNI_LOG(logDEBUG) << "Value = " << value;
+	jint * statusPtr = (jint*)env->GetDirectBufferAddress(status);
+	//DIOJNI_LOG(logDEBUG) << "Status Ptr = " << statusPtr;
+	pulse(*javaId, value, statusPtr);
+	DIOJNI_LOG(logDEBUG) << "Did it work? Status = " << *statusPtr;
 }
 
 /*
@@ -137,9 +150,18 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_pulse
  * Signature: (Ljava/nio/ByteBuffer;Ljava/nio/IntBuffer;)B
  */
 JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isPulsing
-  (JNIEnv *, jclass, jobject, jobject)
+  (JNIEnv *env, jclass, jobject id, jobject status)
 {
-	assert(false);
+	DIOJNI_LOG(logDEBUG) << "Calling DIOJNI isPulsing (RR upd)";
+	void ** javaId = (void**)env->GetDirectBufferAddress(id);
+	//DIOJNI_LOG(logDEBUG) << "Port Ptr = " << *javaId;
+	jint * statusPtr = (jint*)env->GetDirectBufferAddress(status);
+	//DIOJNI_LOG(logDEBUG) << "Status Ptr = " << statusPtr;
+	jbyte returnValue = isPulsing(*javaId, statusPtr);
+	//DIOJNI_LOG(logDEBUG) << "Status = " << *statusPtr;
+	DIOJNI_LOG(logDEBUG) << "isPulsingResult = " << (jbyte)returnValue;
+	return returnValue;
+
 
 }
 
@@ -149,10 +171,14 @@ JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isPulsing
  * Signature: (Ljava/nio/IntBuffer;)B
  */
 JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isAnyPulsing
-  (JNIEnv *, jclass, jobject)
+  (JNIEnv *env, jclass, jobject status)
 {
-	assert(false);
-
+	DIOJNI_LOG(logDEBUG) << "Calling DIOJNI isAnyPulsing (RR upd)";
+	jint * statusPtr = (jint*)env->GetDirectBufferAddress(status);
+	jbyte returnValue = isAnyPulsing( statusPtr );
+	//DIOJNI_LOG(logDEBUG) << "Status = " << *statusPtr;
+	DIOJNI_LOG(logDEBUG) << "isAnyPulsingResult = " << (jbyte)returnValue;
+	return returnValue;
 }
 
 /*

wpilibj/wpilibJavaJNI/lib/EncoderJNI.cpp

@@ -223,3 +223,24 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderSampl
 	ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = " <<  returnValue;
 	return returnValue;
 }
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_EncoderJNI
+ * Method:    setEncoderIndexSource
+ * Signature: (Ljava/nio/ByteBuffer;IZZZLjava/nio/IntBuffer;)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource
+  (JNIEnv * env, jclass, jobject id, jint pin, jboolean analogTrigger, jboolean activeHigh, jboolean edgeSensitive, jobject status)
+{
+	ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderIndexSource";
+	void ** javaId = (void**)env->GetDirectBufferAddress(id);
+	ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << *javaId;
+	jint * statusPtr = (jint*)env->GetDirectBufferAddress(status);
+	ENCODERJNI_LOG(logDEBUG) << "Pin = " << pin;
+	ENCODERJNI_LOG(logDEBUG) << "Analog Trigger = " << (analogTrigger?"true":"false");
+	ENCODERJNI_LOG(logDEBUG) << "Active High = " << (activeHigh?"true":"false");
+	ENCODERJNI_LOG(logDEBUG) << "Edge Sensitive = " << (edgeSensitive?"true":"false");
+	ENCODERJNI_LOG(logDEBUG) << "Status Ptr = " << statusPtr;
+	setEncoderIndexSource(*javaId, pin, analogTrigger, activeHigh, edgeSensitive, statusPtr);
+	ENCODERJNI_LOG(logDEBUG) << "Status = " << *statusPtr;
+}

wpilibj/wpilibJavaJNI/nivision/dxattr.h

@@ -0,0 +1,27 @@
+
+IMAQdxError NI_FUNC IMAQdxGetAttributeU32(IMAQdxSession id, const char* name, uInt32* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeI64(IMAQdxSession id, const char* name, Int64* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeF64(IMAQdxSession id, const char* name, float64* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeString(IMAQdxSession id, const char* name, char value[IMAQDX_MAX_API_STRING_LENGTH]);
+IMAQdxError NI_FUNC IMAQdxGetAttributeEnum(IMAQdxSession id, const char* name, IMAQdxEnumItem* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeBool(IMAQdxSession id, const char* name, bool32* value);
+
+IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumU32(IMAQdxSession id, const char* name, uInt32* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumI64(IMAQdxSession id, const char* name, Int64* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumF64(IMAQdxSession id, const char* name, float64* value);
+
+IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumU32(IMAQdxSession id, const char* name, uInt32* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumI64(IMAQdxSession id, const char* name, Int64* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumF64(IMAQdxSession id, const char* name, float64* value);
+
+IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementU32(IMAQdxSession id, const char* name, uInt32* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementI64(IMAQdxSession id, const char* name, Int64* value);
+IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementF64(IMAQdxSession id, const char* name, float64* value);
+
+IMAQdxError NI_FUNC IMAQdxSetAttributeU32(IMAQdxSession id, const char* name, uInt32 value);
+IMAQdxError NI_FUNC IMAQdxSetAttributeI64(IMAQdxSession id, const char* name, Int64 value);
+IMAQdxError NI_FUNC IMAQdxSetAttributeF64(IMAQdxSession id, const char* name, float64 value);
+IMAQdxError NI_FUNC IMAQdxSetAttributeString(IMAQdxSession id, const char* name, const char* value);
+IMAQdxError NI_FUNC IMAQdxSetAttributeEnum(IMAQdxSession id, const char* name, const IMAQdxEnumItem* value);
+IMAQdxError NI_FUNC IMAQdxSetAttributeBool(IMAQdxSession id, const char* name, bool32 value);
+

wpilibj/wpilibJavaJNI/nivision/dxattr.ini

@@ -0,0 +1,5 @@
+[IMAQdxGetAttributeString]
+outparams=value
+
+[Block Comment]
+exclude=

wpilibj/wpilibJavaJNI/nivision/dxattr_funcs.txt

@@ -0,0 +1,21 @@
+IMAQdxGetAttributeU32
+IMAQdxGetAttributeI64
+IMAQdxGetAttributeF64
+IMAQdxGetAttributeString
+IMAQdxGetAttributeEnum
+IMAQdxGetAttributeBool
+IMAQdxGetAttributeMinimumU32
+IMAQdxGetAttributeMinimumI64
+IMAQdxGetAttributeMinimumF64
+IMAQdxGetAttributeMaximumU32
+IMAQdxGetAttributeMaximumI64
+IMAQdxGetAttributeMaximumF64
+IMAQdxGetAttributeIncrementU32
+IMAQdxGetAttributeIncrementI64
+IMAQdxGetAttributeIncrementF64
+IMAQdxSetAttributeU32
+IMAQdxSetAttributeI64
+IMAQdxSetAttributeF64
+IMAQdxSetAttributeString
+IMAQdxSetAttributeEnum
+IMAQdxSetAttributeBool

wpilibj/wpilibJavaJNI/nivision/gen_java.py

@@ -1,4 +1,5 @@
 from __future__ import print_function
+import codecs
 import sys
 import os
 import re
@@ -9,15 +10,18 @@ except ImportError:
 
 from nivision_parse import *
 
+# base, cast-out-pre, cast-out-post, cast-in-pre, cast-in-post
 java_accessor_map = {
-        "B": "",
-        "C": "Char",
-        "S": "Short",
-        "I": "Int",
-        "J": "Long",
-        "F": "Float",
-        "D": "Double",
-        "Z": "Boolean",
+        "B": ("", "", "", "", ""),
+        "C": ("Char", "", "", "", ""),
+        "S": ("Short", "", "", "", ""),
+        "I": ("Int", "", "", "", ""),
+        "J": ("Long", "", "", "", ""),
+        "F": ("Float", "", "", "", ""),
+        "D": ("Double", "", "", "", ""),
+        "Z": ("Boolean", "", "", "", ""),
+        "X": ("", "(short)(", " & 0xff)", "(byte)(", " & 0xff)"),
+        "Y": ("Short", "(int)(", " & 0xffff)", "(short)(", " & 0xffff)"),
         }
 
 java_size_map = {
@@ -63,9 +67,9 @@ java_types_map = {
         ("int", None): JavaType("int", "int", "jint", "I"),
         ("char", None): JavaType("byte", "byte", "jbyte", "B"),
         ("wchar_t", None): JavaType("char", "char", "jchar", "C"),
-        ("unsigned char", None): JavaType("short", "short", "jshort", "S"),
+        ("unsigned char", None): JavaType("short", "short", "jshort", "X"),
         ("short", None): JavaType("short", "short", "jshort", "S"),
-        ("unsigned short", None): JavaType("int", "int", "jint", "I"),
+        ("unsigned short", None): JavaType("int", "int", "jint", "Y"),
         ("unsigned", None): JavaType("int", "int", "jint", "I"),
         ("unsigned int", None): JavaType("int", "int", "jint", "I"),
         ("uInt32", None): JavaType("int", "int", "jint", "I"),
@@ -431,8 +435,8 @@ structEmit.toArg = "{fname}.getAddress()"
 
 # java type
 jtypeEmit = JavaEmitData()
-jtypeEmit.addBackingRead("{fname} = {backing}.get{jaccessor}({foffset});")
-jtypeEmit.addBackingWrite("{backing}.put{jaccessor}({foffset}, {fname});")
+jtypeEmit.addBackingRead("{fname} = {jaccessor_cast_out_pre}{backing}.get{jaccessor}({foffset}){jaccessor_cast_out_post};")
+jtypeEmit.addBackingWrite("{backing}.put{jaccessor}({foffset}, {jaccessor_cast_in_pre}{fname}{jaccessor_cast_in_post});")
 
 # string - array of characters
 strSizedEmit = JavaEmitData()
@@ -561,7 +565,7 @@ class JavaStructEmitHelper:
         struct_sz = None
         buftype = "ByteBuffer"
         array_size = jtype.array_size or ""
-        jaccessor = None
+        jaccessor = (None, None, None, None, None)
 
         writeBufs = []
         backingRead = []
@@ -612,7 +616,7 @@ for (int i=0, off={foffset}; i<%s; i++, off += {struct_sz})
                 elif jtype.jni_sig[1] == 'B':
                     typeemit = byteArrayEmit
                 elif jtype.jni_sig[1] in java_accessor_map:
-                    buftype = "%sBuffer" % java_accessor_map[jtype.jni_sig[1]]
+                    buftype = "%sBuffer" % java_accessor_map[jtype.jni_sig[1]][0]
                     struct_sz = java_size_map[jtype.jni_sig[1]]
                     typeemit = jtypeArrayEmit
                 else:
@@ -660,7 +664,11 @@ for (int i=0, off={foffset}; i<%s; i++, off += {struct_sz})
                      struct_sz=struct_sz,
                      array_size=array_size,
                      buftype=buftype,
-                     jaccessor=jaccessor,
+                     jaccessor=jaccessor[0],
+                     jaccessor_cast_out_pre=jaccessor[1],
+                     jaccessor_cast_out_post=jaccessor[2],
+                     jaccessor_cast_in_pre=jaccessor[3],
+                     jaccessor_cast_in_post=jaccessor[4],
                      backing=backing)
         jconstruct = [x.format(**fargs) for x in construct]
         jwritebufs = [x.format(**fargs) for x in writeBufs]
@@ -836,15 +844,17 @@ for (int i=0, off={foffset}; i<%s; i++, off += {struct_sz})
 
 
 class JavaEmitter:
-    def __init__(self, outdir, config, config_struct):
+    def __init__(self, outdir, config, config_struct, library_funcs):
         self.outdir = outdir
         self.config = config
         self.config_struct = config_struct
+        self.library_funcs = library_funcs
         self.package = "com.ni.vision"
         self.classname = "NIVision"
         self.classpath = self.package.replace(".", "/") + "/" + self.classname
 
         self.unions = {}
+        self.errors = {}
 
         with open(os.path.join(outdir, "VisionException.java"), "wt") as f:
             print("""//
@@ -922,21 +932,29 @@ public class {classname} {{
     }}
 
     public static ByteBuffer sliceByteBuffer(ByteBuffer bb, int offset, int size) {{
-        ByteBuffer new_bb = bb.duplicate();
-        new_bb.position(offset);
-        new_bb.limit(size);
+        int pos = bb.position();
+        int lim = bb.limit();
+        bb.position(offset);
+        bb.limit(offset+size);
+        ByteBuffer new_bb = bb.slice().order(ByteOrder.nativeOrder());
+        bb.position(pos);
+        bb.limit(lim);
         return new_bb;
     }}
 
     public static ByteBuffer getBytes(ByteBuffer bb, byte[] dst, int offset, int size) {{
-        for (int i=offset; i<offset+size; i++)
-            dst[i] = bb.get(i);
+        int pos = bb.position();
+        bb.position(offset);
+        bb.get(dst, 0, size);
+        bb.position(pos);
         return bb;
     }}
 
     public static ByteBuffer putBytes(ByteBuffer bb, byte[] src, int offset, int size) {{
-        for (int i=offset; i<offset+size; i++)
-            bb.put(i, src[i]);
+        int pos = bb.position();
+        bb.position(offset);
+        bb.put(src, 0, size);
+        bb.position(pos);
         return bb;
     }}
 
@@ -1125,14 +1143,15 @@ public class {classname} {{
 #include <nivision.h>
 #include <NIIMAQdx.h>
 
+static const char* getErrorText(int err);
+
 // throw java exception
 static void throwJavaException(JNIEnv *env) {{
     jclass je = env->FindClass("{packagepath}/VisionException");
     int err = imaqGetLastError();
-    char* err_text = imaqGetErrorText(err);
+    const char* err_text = getErrorText(err);
     char* full_err_msg = (char*)malloc(30+strlen(err_text));
     sprintf(full_err_msg, "imaqError: %d: %s", err, err_text);
-    imaqDispose(err_text);
     env->ThrowNew(je, full_err_msg);
     free(full_err_msg);
 }}
@@ -1140,11 +1159,9 @@ static void throwJavaException(JNIEnv *env) {{
 // throw IMAQdx java exception
 static void dxthrowJavaException(JNIEnv *env, IMAQdxError err) {{
     jclass je = env->FindClass("{packagepath}/VisionException");
-    char* err_text = (char*)malloc(200);
-    IMAQdxGetErrorString(err, err_text, 200);
-    char* full_err_msg = (char*)malloc(250);
+    const char* err_text = getErrorText(err);
+    char* full_err_msg = (char*)malloc(30+strlen(err_text));
     sprintf(full_err_msg, "IMAQdxError: %d: %s", err, err_text);
-    free(err_text);
     env->ThrowNew(je, full_err_msg);
     free(full_err_msg);
 }}
@@ -1154,7 +1171,97 @@ extern "C" {{
 JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass , jlong addr)
 {{
     imaqDispose((void*)addr);
-}}""".format(packagepath=self.package.replace(".", "/"),
+}}
+
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeU32(IMAQdxSession id, const char* name, uInt32* value)
+{{
+    return IMAQdxGetAttribute(id, name, IMAQdxValueTypeU32, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeI64(IMAQdxSession id, const char* name, Int64* value)
+{{
+    return IMAQdxGetAttribute(id, name, IMAQdxValueTypeI64, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeF64(IMAQdxSession id, const char* name, float64* value)
+{{
+    return IMAQdxGetAttribute(id, name, IMAQdxValueTypeF64, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeString(IMAQdxSession id, const char* name, char value[IMAQDX_MAX_API_STRING_LENGTH])
+{{
+    return IMAQdxGetAttribute(id, name, IMAQdxValueTypeString, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeEnum(IMAQdxSession id, const char* name, IMAQdxEnumItem* value)
+{{
+    return IMAQdxGetAttribute(id, name, IMAQdxValueTypeEnumItem, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeBool(IMAQdxSession id, const char* name, bool32* value)
+{{
+    return IMAQdxGetAttribute(id, name, IMAQdxValueTypeBool, (void*)value);
+}}
+
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumU32(IMAQdxSession id, const char* name, uInt32* value)
+{{
+    return IMAQdxGetAttributeMinimum(id, name, IMAQdxValueTypeU32, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumI64(IMAQdxSession id, const char* name, Int64* value)
+{{
+    return IMAQdxGetAttributeMinimum(id, name, IMAQdxValueTypeI64, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumF64(IMAQdxSession id, const char* name, float64* value)
+{{
+    return IMAQdxGetAttributeMinimum(id, name, IMAQdxValueTypeF64, (void*)value);
+}}
+
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumU32(IMAQdxSession id, const char* name, uInt32* value)
+{{
+    return IMAQdxGetAttributeMaximum(id, name, IMAQdxValueTypeU32, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumI64(IMAQdxSession id, const char* name, Int64* value)
+{{
+    return IMAQdxGetAttributeMaximum(id, name, IMAQdxValueTypeI64, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumF64(IMAQdxSession id, const char* name, float64* value)
+{{
+    return IMAQdxGetAttributeMaximum(id, name, IMAQdxValueTypeF64, (void*)value);
+}}
+
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementU32(IMAQdxSession id, const char* name, uInt32* value)
+{{
+    return IMAQdxGetAttributeIncrement(id, name, IMAQdxValueTypeU32, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementI64(IMAQdxSession id, const char* name, Int64* value)
+{{
+    return IMAQdxGetAttributeIncrement(id, name, IMAQdxValueTypeI64, (void*)value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementF64(IMAQdxSession id, const char* name, float64* value)
+{{
+    return IMAQdxGetAttributeIncrement(id, name, IMAQdxValueTypeF64, (void*)value);
+}}
+
+static inline IMAQdxError NI_FUNC IMAQdxSetAttributeU32(IMAQdxSession id, const char* name, uInt32 value)
+{{
+    return IMAQdxSetAttribute(id, name, IMAQdxValueTypeU32, value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxSetAttributeI64(IMAQdxSession id, const char* name, Int64 value)
+{{
+    return IMAQdxSetAttribute(id, name, IMAQdxValueTypeI64, value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxSetAttributeF64(IMAQdxSession id, const char* name, float64 value)
+{{
+    return IMAQdxSetAttribute(id, name, IMAQdxValueTypeF64, value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxSetAttributeString(IMAQdxSession id, const char* name, const char* value)
+{{
+    return IMAQdxSetAttribute(id, name, IMAQdxValueTypeString, value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxSetAttributeEnum(IMAQdxSession id, const char* name, const IMAQdxEnumItem* value)
+{{
+    return IMAQdxSetAttribute(id, name, IMAQdxValueTypeU32, value->Value);
+}}
+static inline IMAQdxError NI_FUNC IMAQdxSetAttributeBool(IMAQdxSession id, const char* name, bool32 value)
+{{
+    return IMAQdxSetAttribute(id, name, IMAQdxValueTypeBool, value);
+}}
+""".format(packagepath=self.package.replace(".", "/"),
            package=self.package.replace(".", "_"),
            classname=self.classname), file=self.outc)
 
@@ -1164,7 +1271,14 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
 
     def finish(self):
         print("}", file=self.out)
-        print("}", file=self.outc)
+        print("""}}
+
+static const char* getErrorText(int err) {{
+    switch (err) {{
+        {errs}
+        default: return "Unknown error";
+    }}
+}}""".format(errs="\n        ".join('case %s: return "%s";' % (x, self.errors[x]) for x in sorted(self.errors))), file=self.outc)
 
     def config_get(self, section, option, fallback):
         try:
@@ -1202,8 +1316,6 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
             return
         if name in opaque_structs:
             return
-        if name.startswith("ERR_"):
-            return
         clean = None
         type = None
         after_struct = False
@@ -1236,6 +1348,10 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
             print("Invalid #define: %s" % name)
             return
 
+        if name.startswith("ERR_"):
+            self.errors[name] = comment
+            return
+
         # strip IMAQ_ prefix
         if name.startswith("IMAQ_"):
             name = name[5:]
@@ -1277,22 +1393,32 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
         for vname, value, comment in values:
             if vname.endswith("SIZE_GUARD"):
                 continue
+            if value is None:
+                # auto-increment
+                value = "%d" % (prev_value + 1)
+            value_i = int(value, 0)
+            if value_i < 0 or value_i != (prev_value + 1):
+                # need to do search instead of index for fromValue()
+                need_search = True
+            prev_value = value_i
+
+            if vname == "IMAQdxErrorSuccess":
+                continue
+            if vname.startswith("IMAQdxError"):
+                self.errors[vname] = comment
+                continue
+
             if vname.startswith("IMAQ_"):
                 vname = vname[5:]
             if vname.startswith("IMAQdx"):
                 vname = vname[6:]
             if vname[0] in "0123456789":
                 vname = "C" + vname
-            if value is None:
-                # auto-increment
-                value = "%d" % (prev_value + 1)
             valuestrs.append("%s(%s),%s" % (vname, value, " // %s" % comment if comment else ""))
             defined.add(vname)
-            value_i = int(value, 0)
-            if value_i < 0 or value_i != (prev_value + 1):
-                # need to do search instead of index for fromValue()
-                need_search = True
-            prev_value = value_i
+
+        if not valuestrs:
+            return
 
         print("""
     public static enum {name} {{
@@ -1346,6 +1472,86 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
         raise NotImplementedError("typedef function not implemented")
 
     def function(self, name, restype, params):
+        if name not in self.library_funcs:
+            return
+        if name == "IMAQdxEnumerateVideoModes":
+            # full custom code
+            print("""
+    public static class dxEnumerateVideoModesResult {{
+        public IMAQdxEnumItem[] videoModeArray;
+        public int currentMode;
+        private ByteBuffer videoModeArray_buf;
+        private dxEnumerateVideoModesResult(ByteBuffer rv_buf, ByteBuffer videoModeArray_buf) {{
+            this.videoModeArray_buf = videoModeArray_buf;
+            int count = rv_buf.getInt(0);
+            videoModeArray = new IMAQdxEnumItem[count];
+            for (int i=0, off=0; i<count; i++, off += {struct_sz}) {{
+                videoModeArray[i] = new IMAQdxEnumItem(videoModeArray_buf, off);
+                videoModeArray[i].read();
+            }}
+            currentMode = rv_buf.getInt(8);
+        }}
+    }}
+
+    public static dxEnumerateVideoModesResult IMAQdxEnumerateVideoModes(int id) {{
+        ByteBuffer rv_buf = ByteBuffer.allocateDirect(8+8).order(ByteOrder.nativeOrder());
+        long rv_addr = getByteBufferAddress(rv_buf);
+        _IMAQdxEnumerateVideoModes(id, 0, rv_addr+0, rv_addr+8);
+        int count = rv_buf.getInt(0);
+        ByteBuffer videoModeArray_buf = ByteBuffer.allocateDirect(count*{struct_sz}).order(ByteOrder.nativeOrder());
+        _IMAQdxEnumerateVideoModes(id, getByteBufferAddress(videoModeArray_buf), rv_addr+0, rv_addr+8);
+        dxEnumerateVideoModesResult rv = new dxEnumerateVideoModesResult(rv_buf, videoModeArray_buf);
+        return rv;
+    }}
+    private static native void _IMAQdxEnumerateVideoModes(int id, long videoModeArray, long count, long currentMode);""".format(struct_sz=self.config_struct.get("IMAQdxEnumItem", "_SIZE_")), file=self.out)
+            print("""
+JNIEXPORT void JNICALL Java_{package}_{classname}__1IMAQdxEnumerateVideoModes(JNIEnv* env, jclass , jint id, jlong videoModeArray, jlong count, jlong currentMode)
+{{
+    IMAQdxError rv = IMAQdxEnumerateVideoModes((IMAQdxSession)id, (IMAQdxVideoMode*)videoModeArray, (uInt32*)count, (uInt32*)currentMode);
+    if (rv != IMAQdxErrorSuccess) dxthrowJavaException(env, rv);
+}}""".format(package=self.package.replace(".", "_"),
+             classname=self.classname), file=self.outc)
+            return
+        elif name == "IMAQdxGetImageData":
+            print("""
+    public static int IMAQdxGetImageData(int id, ByteBuffer buffer, IMAQdxBufferNumberMode mode, int desiredBufferNumber) {{
+        long buffer_addr = getByteBufferAddress(buffer);
+        int buffer_size = buffer.capacity();
+        return _IMAQdxGetImageData(id, buffer_addr, buffer_size, mode.getValue(), desiredBufferNumber);
+    }}
+    private static native int _IMAQdxGetImageData(int id, long buffer, int bufferSize, int mode, int desiredBufferNumber);""".format(), file=self.out)
+            print("""
+JNIEXPORT jint JNICALL Java_{package}_{classname}__1IMAQdxGetImageData(JNIEnv* env, jclass , jint id, jlong buffer, jint bufferSize, jint mode, jint desiredBufferNumber)
+{{
+    uInt32 actualBufferNumber;
+    IMAQdxError rv = IMAQdxGetImageData((IMAQdxSession)id, (void*)buffer, (uInt32)bufferSize, (IMAQdxBufferNumberMode)mode, (uInt32)desiredBufferNumber, &actualBufferNumber);
+    if (rv != IMAQdxErrorSuccess) dxthrowJavaException(env, rv);
+    return (jint)actualBufferNumber;
+}}""".format(package=self.package.replace(".", "_"),
+             classname=self.classname), file=self.outc)
+        elif name == "imaqReadFile":
+            print("""
+    public static void imaqReadFile(Image image, String fileName) {{
+        ByteBuffer fileName_buf;
+        byte[] fileName_bytes;
+        try {{
+            fileName_bytes = fileName.getBytes("UTF-8");
+        }} catch (UnsupportedEncodingException e) {{
+            fileName_bytes = new byte[0];
+        }}
+        fileName_buf = ByteBuffer.allocateDirect(fileName_bytes.length+1);
+        putBytes(fileName_buf, fileName_bytes, 0, fileName_bytes.length).put(fileName_bytes.length, (byte)0);
+        _imaqReadFile(image.getAddress(), getByteBufferAddress(fileName_buf), 0, 0);
+    }}
+    private static native void _imaqReadFile(long image, long fileName, long colorTable, long numColors);""".format(), file=self.out)
+            print("""
+JNIEXPORT void JNICALL Java_{package}_{classname}__1imaqReadFile(JNIEnv* env, jclass , jlong image, jlong fileName, jlong colorTable, jlong numColors)
+{{
+    int rv = imaqReadFile((Image*)image, (const char*)fileName, (RGBValue*)colorTable, (int*)numColors);
+    if (rv == 0) throwJavaException(env);
+}}""".format(package=self.package.replace(".", "_"),
+             classname=self.classname), file=self.outc)
+            return
         if self.config_getboolean(name, "exclude", fallback=False):
             return
 
@@ -1443,8 +1649,10 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
             is_nullok = (fname in nullokparams) and not is_outparam
 
             if is_outparam:
-                assert ftype[-1] == '*'
-                ftype = ftype[:-1]
+                if ftype[-1] == '*':
+                    ftype = ftype[:-1]
+                elif arr is None:
+                    raise ValueError("outparam %s is not a pointer or array", fname)
             field = helper.get_field_java_code(fname, ftype, arr, 0, jfielddefs_private, backing="%s_buf" % fname)
             paramtypes[fname] = (ftype, arr, field["type"])
             if is_outparam:
@@ -1510,17 +1718,29 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
         outstruct_name = None
         #print(name, jrettype, outparams, retarraysize, retsize)
         if outparams or retarraysize or retsize:
-            # create a return buffer (TODO: optimize size)
-            jinit.append("ByteBuffer rv_buf = ByteBuffer.allocateDirect(%d).order(ByteOrder.nativeOrder());" % ((len(outparams)+1)*8))
-            jinit.append("long rv_addr = getByteBufferAddress(rv_buf);")
-
             # create a return structure
-            outstruct_fields = [(pname, ptype[:-1], arr, "")
-                                for (pname, ptype, arr) in params
-                                if pname in outparams]
+            outstruct_fields = []
+            outstruct_size = []
+            for (pname, ptype, arr) in params:
+                if pname not in outparams:
+                    continue
+                if ptype[-1] == '*':
+                    ptype = ptype[:-1]
+                outstruct_fields.append((pname, ptype, arr, ""))
+                if arr:
+                    if ptype == "char":
+                        outstruct_size.append(arr)
+                    else:
+                        raise NotImplementedError("non-char array")
+                else:
+                    outstruct_size.append("8")
             outstruct_sized_members = {}
             outstruct_name = name[4:] + "Result"
 
+            # create a return buffer (TODO: optimize size)
+            jinit.append("ByteBuffer rv_buf = ByteBuffer.allocateDirect(%s).order(ByteOrder.nativeOrder());" % "+".join(outstruct_size))
+            jinit.append("long rv_addr = getByteBufferAddress(rv_buf);")
+
             jconstruct_args = [("rv_buf", "ByteBuffer")]
             jconstruct = []
             if retarraysize:
@@ -1568,7 +1788,7 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
                 jfini.extend(jconstruct)
                 jretc = "return %s;" % outparams[0]
                 jrettype = paramtypes[outparams[0]][2].j_type
-                rettype = paramtypes[outparams[0]][2]
+                #rettype = paramtypes[outparams[0]][2]
             elif len(outparams) == 1 and retsize:
                 jfini.extend(x.replace("public ", "") for x in jfielddefs)
                 jfini.extend(jconstruct)
@@ -1671,6 +1891,8 @@ JNIEXPORT void JNICALL Java_{package}_{classname}_imaqDispose(JNIEnv* , jclass ,
                 cargs.append("*((%s*)%s)" % (ptype, pname))
             elif ptype.endswith("String255"):
                 cargs.append("(char *)%s" % pname)
+            elif arr:
+                cargs.append("(%s*)%s" % (ptype, pname))
             else:
                 cargs.append("(%s)%s" % (ptype, pname))
 
@@ -1711,7 +1933,7 @@ JNIEXPORT {rettype} JNICALL Java_{package}_{classname}__1{name}({args})
 def generate(srcdir, outdir, inputs):
     emit = None
 
-    for fname, config_struct_path, configpath in inputs:
+    for fname, config_struct_path, configpath, funcs_path in inputs:
         # read config files
         config_struct = configparser.ConfigParser()
         config_struct.read(config_struct_path)
@@ -1720,17 +1942,23 @@ def generate(srcdir, outdir, inputs):
         block_comment_exclude = set(x.strip() for x in
                 config.get("Block Comment", "exclude").splitlines())
 
+        library_funcs = set()
+        with open(funcs_path) as ff:
+            for line in ff:
+                library_funcs.add(line.strip())
+
         # open input file
-        with open(fname) as inf:
+        with codecs.open(fname, encoding="utf-8", errors="ignore") as inf:
             # prescan for undefined structures
             prescan_file(inf)
             inf.seek(0)
 
             if emit is None:
-                emit = JavaEmitter(outdir, config, config_struct)
+                emit = JavaEmitter(outdir, config, config_struct, library_funcs)
             else:
                 emit.config = config
                 emit.config_struct = config_struct
+                emit.library_funcs = library_funcs
 
             # generate
             parse_file(emit, inf, block_comment_exclude)
@@ -1738,15 +1966,16 @@ def generate(srcdir, outdir, inputs):
     emit.finish()
 
 if __name__ == "__main__":
-    if len(sys.argv) < 4 or ((len(sys.argv)-1) % 3) != 0:
-        print("Usage: gen_wrap.py <header.h config_struct.ini config.ini>...")
+    if len(sys.argv) < 5 or ((len(sys.argv)-1) % 4) != 0:
+        print("Usage: gen_wrap.py <header.h config_struct.ini config.ini funcs.txt>...")
         exit(0)
 
     inputs = []
-    for i in range(1, len(sys.argv), 3):
+    for i in range(1, len(sys.argv), 4):
         fname = sys.argv[i]
         config_struct_name = sys.argv[i+1]
         configname = sys.argv[i+2]
-        inputs.append((fname, config_struct_name, configname))
+        funcs_name = sys.argv[i+3]
+        inputs.append((fname, config_struct_name, configname, funcs_name))
 
     generate("", "", inputs)

wpilibj/wpilibJavaJNI/nivision/generateJNI.sh

@@ -13,15 +13,28 @@ python gen_struct_sizer.py ../../../wpilibc/wpilibC++Devices/include/NIIMAQdx.h
 arm-frc-linux-gnueabi-gcc -I../../../wpilibc/wpilibC++Devices/include -S struct_sizer.c
 cat struct_sizer.s | python get_struct_size.py > imaqdx_arm.ini
 
+# Get functions actually in the .so; some functions are in the header but
+# not the shared library!
+arm-frc-linux-gnueabi-nm -D ../../../ni-libraries/libnivision.so.14.0.0 | cut -c 12- | grep ^imaq > nivision_funcs.txt
+echo Priv_ReadJPEGString_C >> nivision_funcs.txt
+arm-frc-linux-gnueabi-nm -D ../../../ni-libraries/libniimaqdx.so.14.0.0 | cut -c 12- | grep ^IMAQdx > imaqdx_funcs.txt
+
 # Run python generator.
 python gen_java.py \
     ../../../wpilibc/wpilibC++Devices/include/nivision.h \
     nivision_arm.ini \
     nivision_2011.ini \
+    nivision_funcs.txt \
     \
     ../../../wpilibc/wpilibC++Devices/include/NIIMAQdx.h \
     imaqdx_arm.ini \
-    imaqdx.ini
+    imaqdx.ini \
+    imaqdx_funcs.txt \
+    \
+    dxattr.h \
+    imaqdx_arm.ini \
+    dxattr.ini \
+    dxattr_funcs.txt
 
 # Stick generated files into appropriate places.
 cp NIVision.cpp ../lib/NIVisionJNI.cpp

wpilibj/wpilibJavaJNI/nivision/imaqdx.ini

@@ -7,7 +7,6 @@ exclude=
 
 ; Error Codes Enumeration
 [IMAQdxError]
-exclude=True
 
 ; Callbacks
 [FrameDoneEventCallbackPtr]

wpilibj/wpilibJavaJNI/nivision/nivision_2011.ini

@@ -265,6 +265,7 @@ arraysize=measurements:numMeasurements
 [imaqParticleFilter4]
 arraysize=criteria:criteriaCount
 outparams=numParticles
+nullok=roi
 
 ; Morphology functions
 [imaqFindCircles]
@@ -391,7 +392,8 @@ size=data:dataSize
 # unclear whether dataSize is input or output parameter
 exclude=True
 [imaqReadFile]
-nullok=colorTable,numColors
+;nullok=colorTable,numColors
+;arraysize=colorTable:numColors
 [imaqWriteAVIFrame]
 size=data:dataLength
 [imaqWriteBMPFile]