Merge pull request #2 from Team766/fix_wpirobotprovider_hasnewdrivers…

…tationdata

Provide new implementation for WPIRobotProvider.hasNewDriverStationData() and update integration in OI

src/main/java/com/team766/hal/RobotProvider.java

@@ -230,5 +230,7 @@ public GyroReader getGyro(String configName) {
 
 	public abstract double getBatteryVoltage();
 
+	public abstract void refreshDriverStationData();
+
 	public abstract boolean hasNewDriverStationData();
 }

src/main/java/com/team766/hal/mock/TestRobotProvider.java

@@ -107,6 +107,11 @@ public Clock getClock() {
 		return SystemClock.instance;
 	}
 
+	@Override
+	public void refreshDriverStationData() {
+		// no-op
+	}
+
 	@Override
 	public boolean hasNewDriverStationData() {
 		boolean result = m_hasDriverStationUpdate;

src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java

@@ -104,6 +104,12 @@ public Clock getClock() {
 		return SimulationClock.instance;
 	}
 
+	@Override
+	public void refreshDriverStationData() {
+		// no-op on simulator
+		return;
+	}
+
 	@Override
 	public boolean hasNewDriverStationData() {
 		int newUpdateNumber = ProgramInterface.driverStationUpdateNumber;

src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java

@@ -1,5 +1,7 @@
 package com.team766.hal.wpilib;
 
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
 import com.team766.hal.AnalogInputReader;
 import com.team766.hal.CameraInterface;
 import com.team766.hal.CameraReader;
@@ -22,7 +24,9 @@
 import com.team766.logging.Logger;
 import com.team766.logging.LoggerExceptionUtils;
 import com.team766.logging.Severity;
-
+import com.team766.simulator.elements.AirCompressor;
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.PneumaticsControlModule;
@@ -31,14 +35,73 @@
 
 public class WPIRobotProvider extends RobotProvider {
 
-	private MotorController[][] motors = new MotorController[MotorController.Type.values().length][64];
+	/**
+	 * Runnable that counts the number of times we receive new data from the driver station. Used as
+	 * part of impl of {@link #hasNewDriverStationData()}.
+	 */
+	private static class DataRefreshRunnable implements Runnable {
+		private final AtomicBoolean m_keepAlive = new AtomicBoolean();
+		private final AtomicInteger m_dataCount = new AtomicInteger();
+
+		public DataRefreshRunnable() {
+			m_keepAlive.set(true);
+		}
+
+		public void cancel() {
+			m_keepAlive.set(false);
+		}
+
+		@Override
+		public void run() {
+			// create and register a handle that gets notified whenever there's new DS data.
+			int handle = WPIUtilJNI.createEvent(false, false);
+			DriverStationJNI.provideNewDataEventHandle(handle);
+
+			while (m_keepAlive.get()) {
+				try {
+					// wait for new data or timeout
+					// (timeout returns true)
+					if (!WPIUtilJNI.waitForObjectTimeout(handle, 0.1)) {
+						m_dataCount.incrementAndGet();
+					}
+				} catch (InterruptedException e) {
+					// should only happen during failures
+					LoggerExceptionUtils.logException(e);
+
+					// clean up handle
+					DriverStationJNI.removeNewDataEventHandle(handle);
+					WPIUtilJNI.destroyEvent(handle);
+
+					// re-register handle in an attempt to keep data flowing.
+					handle = WPIUtilJNI.createEvent(false, false);
+					DriverStationJNI.provideNewDataEventHandle(handle);
+				}
+			}
+
+			DriverStationJNI.removeNewDataEventHandle(handle);
+			WPIUtilJNI.destroyEvent(handle);
+		}
+	}
+
+	private DataRefreshRunnable m_DataRefreshRunnable = new DataRefreshRunnable();
+	private Thread m_dataRefreshThread;
+	private int m_lastDataCount = 0;
+
+	public WPIRobotProvider() {
+		m_dataRefreshThread = new Thread(m_DataRefreshRunnable, "DataRefreshThread");
+		m_dataRefreshThread.start();
+	}
+
+	private MotorController[][] motors =
+			new MotorController[MotorController.Type.values().length][64];
 
 	// The presence of this object allows the compressor to run before we've declared any solenoids.
 	@SuppressWarnings("unused")
 	private PneumaticsControlModule pcm = new PneumaticsControlModule();
 
 	@Override
-	public MotorController getMotor(int index, String configPrefix, MotorController.Type type, ControlInputReader localSensor) {
+	public MotorController getMotor(int index, String configPrefix, MotorController.Type type,
+			ControlInputReader localSensor) {
 		if (motors[type.ordinal()][index] != null) {
 			return motors[type.ordinal()][index];
 		}
@@ -49,7 +112,8 @@ public MotorController getMotor(int index, String configPrefix, MotorController.
 					motor = new CANSparkMaxMotorController(index);
 				} catch (Exception ex) {
 					LoggerExceptionUtils.logException(ex);
-					motor = new LocalMotorController(configPrefix, new MockMotorController(index), localSensor);
+					motor = new LocalMotorController(configPrefix, new MockMotorController(index),
+							localSensor);
 					localSensor = null;
 				}
 				break;
@@ -68,8 +132,10 @@ public MotorController getMotor(int index, String configPrefix, MotorController.
 				break;
 		}
 		if (motor == null) {
-			LoggerExceptionUtils.logException(new IllegalArgumentException("Unsupported motor type " + type));
-			motor = new LocalMotorController(configPrefix, new MockMotorController(index), localSensor);
+			LoggerExceptionUtils
+					.logException(new IllegalArgumentException("Unsupported motor type " + type));
+			motor = new LocalMotorController(configPrefix, new MockMotorController(index),
+					localSensor);
 			localSensor = null;
 		}
 		if (localSensor != null) {
@@ -81,34 +147,31 @@ public MotorController getMotor(int index, String configPrefix, MotorController.
 
 	@Override
 	public EncoderReader getEncoder(int index1, int index2) {
-		if(encoders[index1] == null)
+		if (encoders[index1] == null)
 			encoders[index1] = new Encoder(index1, index2);
 		return encoders[index1];
 	}
 
 	@Override
 	public SolenoidController getSolenoid(int index) {
-		if(solenoids[index] == null)
+		if (solenoids[index] == null)
 			solenoids[index] = new Solenoid(index);
 		return solenoids[index];
 	}
 
 	@Override
-	//Gyro index values:
-	// -1 	= Spartan Gyro
-	// 	0+ 	= Analog Gyro on port index
+	// Gyro index values:
+	// -1 = Spartan Gyro
+	// 0+ = Analog Gyro on port index
 	public GyroReader getGyro(int index) {
-		if(gyros[index + 2] == null){
-			if(index < -2) {
-				Logger.get(Category.CONFIGURATION).logRaw(
-					Severity.ERROR,
-					"Invalid gyro port " + index + ". Must be -2, -1, or a non-negative integer"
-				);
+		if (gyros[index + 2] == null) {
+			if (index < -2) {
+				Logger.get(Category.CONFIGURATION).logRaw(Severity.ERROR, "Invalid gyro port "
+						+ index + ". Must be -2, -1, or a non-negative integer");
 				return new MockGyro();
-			}
-			else if(index == -2)
+			} else if (index == -2)
 				gyros[index + 2] = new NavXGyro(I2C.Port.kOnboard);
-			else if(index == -1)
+			else if (index == -1)
 				gyros[index + 2] = new ADXRS450_Gyro(SPI.Port.kOnboardCS0);
 			else
 				gyros[index + 2] = new AnalogGyro(index);
@@ -124,13 +187,13 @@ public CameraReader getCamera(String id, String value) {
 
 	@Override
 	public JoystickReader getJoystick(int index) {
-		if(joysticks[index] == null)
+		if (joysticks[index] == null)
 			joysticks[index] = new Joystick(index);
 		return joysticks[index];
 	}
 
 	@Override
-	public CameraInterface getCamServer(){
+	public CameraInterface getCamServer() {
 		return new com.team766.hal.wpilib.CameraInterface();
 	}
 
@@ -142,15 +205,15 @@ public DigitalInputReader getDigitalInput(int index) {
 	}
 
 	@Override
-	public AnalogInputReader getAnalogInput(int index){
-		if(angInputs[index] == null)
+	public AnalogInputReader getAnalogInput(int index) {
+		if (angInputs[index] == null)
 			angInputs[index] = new AnalogInput(index);
 		return angInputs[index];
 	}
 
 	@Override
 	public RelayOutput getRelay(int index) {
-		if(relays[index] == null)
+		if (relays[index] == null)
 			relays[index] = new Relay(index);
 		return relays[index];
 	}
@@ -159,10 +222,8 @@ public RelayOutput getRelay(int index) {
 	public PositionReader getPositionSensor() {
 		if (positionSensor == null) {
 			positionSensor = new MockPositionSensor();
-			Logger.get(Category.CONFIGURATION).logRaw(
-				Severity.ERROR,
-				"Position sensor does not exist on real robots. Using mock position sensor instead - it will always return a position of 0"
-			);
+			Logger.get(Category.CONFIGURATION).logRaw(Severity.ERROR,
+					"Position sensor does not exist on real robots. Using mock position sensor instead - it will always return a position of 0");
 		}
 		return positionSensor;
 	}
@@ -172,15 +233,22 @@ public Clock getClock() {
 		return SystemClock.instance;
 	}
 
+	@Override
+	public void refreshDriverStationData() {
+		DriverStation.refreshData();
+	}
+
 	@Override
 	public boolean hasNewDriverStationData() {
-		// TODO: replace implementation with event counting one
-		// return DriverStation.isNewControlData();
-		return true;
+		// see if the thread has counted more data changes than the last time this method was called
+		int currentDataRefreshThreadCount = m_DataRefreshRunnable.m_dataCount.get();
+		boolean dataChanged = m_lastDataCount != currentDataRefreshThreadCount;
+		m_lastDataCount = currentDataRefreshThreadCount;
+		return dataChanged;
 	}
 
 	@Override
 	public double getBatteryVoltage() {
 		return RobotController.getBatteryVoltage();
 	}
-}
+}

src/main/java/com/team766/robot/OI.java

@@ -6,6 +6,7 @@
 import com.team766.hal.RobotProvider;
 import com.team766.logging.Category;
 import com.team766.robot.procedures.*;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -26,11 +27,12 @@ public OI() {
 
 	public void run(Context context) {
 		while (true) {
+			// wait for driver station data (and refresh it using the WPILib APIs)
+			context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
+			RobotProvider.instance.refreshDriverStationData();
+
 			// Add driver controls here - make sure to take/release ownership
 			// of mechanisms when appropriate.
-
-
-			context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
 		}
 	}
 }