Rename our allwpilib (which is now 2020) to not have 2019 in the name
Change-Id: I3c07f85ed32ab8b97db765a9b43f2a6ce7da964a
diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt
new file mode 100644
index 0000000..7874f7f
--- /dev/null
+++ b/wpilibj/CMakeLists.txt
@@ -0,0 +1,35 @@
+project (wpilibj)
+
+find_package( OpenCV REQUIRED )
+
+# Java bindings
+if (NOT WITHOUT_JAVA)
+ find_package(Java REQUIRED)
+ include(UseJava)
+ set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+
+ set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
+
+ find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
+
+ configure_file(src/generate/WPILibVersion.java.in WPILibVersion.java)
+
+ file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+ file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar")
+ file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+
+ add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpiutil_jar OUTPUT_NAME wpilibj)
+
+ get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE)
+ install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+ set_property(TARGET wpilibj_jar PROPERTY FOLDER "java")
+
+ if (MSVC OR FLAT_INSTALL_WPILIB)
+ set (wpilibj_config_dir ${wpilib_dest})
+ else()
+ set (wpilibj_config_dir share/wpilibj)
+ endif()
+
+ install(FILES wpilibj-config.cmake DESTINATION ${wpilibj_config_dir})
+endif()
diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle
new file mode 100644
index 0000000..b2e22fb
--- /dev/null
+++ b/wpilibj/build.gradle
@@ -0,0 +1,174 @@
+evaluationDependsOn(':hal')
+evaluationDependsOn(':ntcore')
+evaluationDependsOn(':cscore')
+evaluationDependsOn(':cameraserver')
+
+ext {
+ baseId = 'wpilibj'
+ groupId = 'edu.wpi.first.wpilibj'
+ devMain = 'edu.wpi.first.wpilibj.DevMain'
+}
+
+apply from: "${rootDir}/shared/java/javacommon.gradle"
+
+def wpilibVersionFileInput = file("src/generate/WPILibVersion.java.in")
+def wpilibVersionFileOutput = file("$buildDir/generated/java/edu/wpi/first/wpilibj/util/WPILibVersion.java")
+
+task generateJavaVersion() {
+ description = 'Generates the wpilib version class'
+ group = 'WPILib'
+
+ outputs.file wpilibVersionFileOutput
+ inputs.file wpilibVersionFileInput
+
+ if (wpilibVersioning.releaseMode) {
+ outputs.upToDateWhen { false }
+ }
+
+ // We follow a simple set of checks to determine whether we should generate a new version file:
+ // 1. If the release type is not development, we generate a new version file
+ // 2. If there is no generated version number, we generate a new version file
+ // 3. If there is a generated build number, and the release type is development, then we will
+ // only generate if the publish task is run.
+ doLast {
+ def version = wpilibVersioning.version.get()
+ println "Writing version ${version} to $wpilibVersionFileOutput"
+
+ if (wpilibVersionFileOutput.exists()) {
+ wpilibVersionFileOutput.delete()
+ }
+ def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
+ wpilibVersionFileOutput.write(read)
+ }
+}
+
+gradle.taskGraph.addTaskExecutionGraphListener { graph ->
+ def willPublish = graph.hasTask(publish)
+ if (willPublish) {
+ generateJavaVersion.outputs.upToDateWhen { false }
+ }
+}
+
+sourceSets.main.java.srcDir "${buildDir}/generated/java/"
+
+compileJava {
+ dependsOn generateJavaVersion
+}
+
+repositories {
+ jcenter()
+}
+
+dependencies {
+ implementation project(':hal')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':cameraserver')
+ testImplementation 'com.google.guava:guava:19.0'
+ testImplementation 'org.mockito:mockito-core:2.27.0'
+ devImplementation project(':hal')
+ devImplementation project(':wpiutil')
+ devImplementation project(':ntcore')
+ devImplementation project(':cscore')
+ devImplementation project(':cameraserver')
+ devImplementation sourceSets.main.output
+}
+
+apply plugin: 'cpp'
+apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: ExtraTasks
+apply from: "${rootDir}/shared/config.gradle"
+
+project(':').libraryBuild.dependsOn build
+
+ext {
+ sharedCvConfigs = [wpilibjDev: []]
+ staticCvConfigs = [:]
+ useJava = true
+ useCpp = true
+}
+
+apply from: "${rootDir}/shared/opencv.gradle"
+
+model {
+ components {
+ wpilibjDev(NativeExecutableSpec) {
+ targetBuildTypes 'debug'
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/dev/native/cpp'
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/dev/native/include'
+ }
+ }
+ }
+ binaries.all {
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+ lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
+ lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
+ lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ project(':hal').addHalDependency(it, 'shared')
+ project(':hal').addHalJniDependency(it)
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+ }
+ }
+ }
+ }
+ tasks {
+ def c = $.components
+ project.tasks.create('runCpp', Exec) {
+ group = 'WPILib'
+ description = "Run the wpilibjDev executable"
+ def found = false
+ def systemArch = getCurrentArch()
+ c.each {
+ //println it.name
+ if (it in NativeExecutableSpec && it.name == "wpilibjDev") {
+ it.binaries.each {
+ if (!found) {
+ def arch = it.targetPlatform.name
+ if (arch == systemArch) {
+ dependsOn it.tasks.install
+ commandLine it.tasks.install.runScriptFile.get().asFile.toString()
+ def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+ test.dependsOn it.tasks.install
+ test.systemProperty 'java.library.path', filePath
+ test.environment 'LD_LIBRARY_PATH', filePath
+ test.workingDir filePath
+ run.dependsOn it.tasks.install
+ run.systemProperty 'java.library.path', filePath
+ run.environment 'LD_LIBRARY_PATH', filePath
+ run.workingDir filePath
+
+ found = true
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+def oldWpilibVersionFile = file('src/main/java/edu/wpi/first/wpilibj/util/WPILibVersion.java')
+
+clean {
+ delete oldWpilibVersionFile
+}
+
+test {
+ testLogging {
+ outputs.upToDateWhen {false}
+ showStandardStreams = true
+ }
+}
+
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
diff --git a/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java b/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
new file mode 100644
index 0000000..2c08b17
--- /dev/null
+++ b/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpiutil.RuntimeDetector;
+
+public final class DevMain {
+ /**
+ * Main entry point.
+ */
+ public static void main(String[] args) {
+ System.out.println("Hello World!");
+ System.out.println(RuntimeDetector.getPlatformPath());
+ System.out.println(NetworkTablesJNI.now());
+ System.out.println(HALUtil.getHALRuntimeType());
+ }
+
+ private DevMain() {
+ }
+}
diff --git a/wpilibj/src/dev/native/cpp/main.cpp b/wpilibj/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..e324b44
--- /dev/null
+++ b/wpilibj/src/dev/native/cpp/main.cpp
@@ -0,0 +1,8 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+int main() {}
diff --git a/wpilibj/src/generate/WPILibVersion.java.in b/wpilibj/src/generate/WPILibVersion.java.in
new file mode 100644
index 0000000..cc5e6b0
--- /dev/null
+++ b/wpilibj/src/generate/WPILibVersion.java.in
@@ -0,0 +1,10 @@
+package edu.wpi.first.wpilibj.util;
+
+/*
+ * Autogenerated file! Do not manually edit this file. This version is regenerated
+ * any time the publish task is run, or when this file is deleted.
+ */
+
+public final class WPILibVersion {
+ public static final String Version = "${wpilib_version}";
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
new file mode 100644
index 0000000..d901462
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * ADXL345 I2C Accelerometer.
+ */
+@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
+public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
+ private static final byte kAddress = 0x1D;
+ private static final byte kPowerCtlRegister = 0x2D;
+ private static final byte kDataFormatRegister = 0x31;
+ private static final byte kDataRegister = 0x32;
+ private static final double kGsPerLSB = 0.00390625;
+ private static final byte kPowerCtl_Link = 0x20;
+ private static final byte kPowerCtl_AutoSleep = 0x10;
+ private static final byte kPowerCtl_Measure = 0x08;
+ private static final byte kPowerCtl_Sleep = 0x04;
+
+ private static final byte kDataFormat_SelfTest = (byte) 0x80;
+ private static final byte kDataFormat_SPI = 0x40;
+ private static final byte kDataFormat_IntInvert = 0x20;
+ private static final byte kDataFormat_FullRes = 0x08;
+ private static final byte kDataFormat_Justify = 0x04;
+
+ public enum Axes {
+ kX((byte) 0x00),
+ kY((byte) 0x02),
+ kZ((byte) 0x04);
+
+ /**
+ * The integer value representing this enumeration.
+ */
+ @SuppressWarnings("MemberName")
+ public final byte value;
+
+ Axes(byte value) {
+ this.value = value;
+ }
+ }
+
+ @SuppressWarnings("MemberName")
+ public static class AllAxes {
+ public double XAxis;
+ public double YAxis;
+ public double ZAxis;
+ }
+
+ protected I2C m_i2c;
+
+ protected SimDevice m_simDevice;
+ protected SimEnum m_simRange;
+ protected SimDouble m_simX;
+ protected SimDouble m_simY;
+ protected SimDouble m_simZ;
+
+ /**
+ * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
+ *
+ * @param port The I2C port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL345_I2C(I2C.Port port, Range range) {
+ this(port, range, kAddress);
+ }
+
+ /**
+ * Constructs the ADXL345 Accelerometer over I2C.
+ *
+ * @param port The I2C port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
+ */
+ public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
+ m_i2c = new I2C(port, deviceAddress);
+
+ // simulation
+ m_simDevice = SimDevice.create("ADXL345_I2C", port.value, deviceAddress);
+ if (m_simDevice != null) {
+ m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+ }
+
+ // Turn on the measurements
+ m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
+
+ setRange(range);
+
+ HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
+ SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_i2c != null) {
+ m_i2c.close();
+ m_i2c = null;
+ }
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ m_simDevice = null;
+ }
+ }
+
+ @Override
+ public void setRange(Range range) {
+ final byte value;
+
+ switch (range) {
+ case k2G:
+ value = 0;
+ break;
+ case k4G:
+ value = 1;
+ break;
+ case k8G:
+ value = 2;
+ break;
+ case k16G:
+ value = 3;
+ break;
+ default:
+ throw new IllegalArgumentException(range + " unsupported range type");
+ }
+
+ // Specify the data format to read
+ m_i2c.write(kDataFormatRegister, kDataFormat_FullRes | value);
+
+ if (m_simRange != null) {
+ m_simRange.set(value);
+ }
+ }
+
+ @Override
+ public double getX() {
+ return getAcceleration(Axes.kX);
+ }
+
+ @Override
+ public double getY() {
+ return getAcceleration(Axes.kY);
+ }
+
+ @Override
+ public double getZ() {
+ return getAcceleration(Axes.kZ);
+ }
+
+ /**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL345 in Gs.
+ */
+ public double getAcceleration(Axes axis) {
+ if (axis == Axes.kX && m_simX != null) {
+ return m_simX.get();
+ }
+ if (axis == Axes.kY && m_simY != null) {
+ return m_simY.get();
+ }
+ if (axis == Axes.kZ && m_simZ != null) {
+ return m_simZ.get();
+ }
+ ByteBuffer rawAccel = ByteBuffer.allocate(2);
+ m_i2c.read(kDataRegister + axis.value, 2, rawAccel);
+
+ // Sensor is little endian... swap bytes
+ rawAccel.order(ByteOrder.LITTLE_ENDIAN);
+ return rawAccel.getShort(0) * kGsPerLSB;
+ }
+
+ /**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
+ */
+ public AllAxes getAccelerations() {
+ AllAxes data = new AllAxes();
+ if (m_simX != null && m_simY != null && m_simZ != null) {
+ data.XAxis = m_simX.get();
+ data.YAxis = m_simY.get();
+ data.ZAxis = m_simZ.get();
+ return data;
+ }
+ ByteBuffer rawData = ByteBuffer.allocate(6);
+ m_i2c.read(kDataRegister, 6, rawData);
+
+ // Sensor is little endian... swap bytes
+ rawData.order(ByteOrder.LITTLE_ENDIAN);
+ data.XAxis = rawData.getShort(0) * kGsPerLSB;
+ data.YAxis = rawData.getShort(2) * kGsPerLSB;
+ data.ZAxis = rawData.getShort(4) * kGsPerLSB;
+ return data;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ NetworkTableEntry entryX = builder.getEntry("X");
+ NetworkTableEntry entryY = builder.getEntry("Y");
+ NetworkTableEntry entryZ = builder.getEntry("Z");
+ builder.setUpdateTable(() -> {
+ AllAxes data = getAccelerations();
+ entryX.setDouble(data.XAxis);
+ entryY.setDouble(data.YAxis);
+ entryZ.setDouble(data.ZAxis);
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
new file mode 100644
index 0000000..6aed40e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -0,0 +1,247 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * ADXL345 SPI Accelerometer.
+ */
+@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
+public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
+ private static final int kPowerCtlRegister = 0x2D;
+ private static final int kDataFormatRegister = 0x31;
+ private static final int kDataRegister = 0x32;
+ private static final double kGsPerLSB = 0.00390625;
+
+ private static final int kAddress_Read = 0x80;
+ private static final int kAddress_MultiByte = 0x40;
+
+ private static final int kPowerCtl_Link = 0x20;
+ private static final int kPowerCtl_AutoSleep = 0x10;
+ private static final int kPowerCtl_Measure = 0x08;
+ private static final int kPowerCtl_Sleep = 0x04;
+
+ private static final int kDataFormat_SelfTest = 0x80;
+ private static final int kDataFormat_SPI = 0x40;
+ private static final int kDataFormat_IntInvert = 0x20;
+ private static final int kDataFormat_FullRes = 0x08;
+ private static final int kDataFormat_Justify = 0x04;
+
+ public enum Axes {
+ kX((byte) 0x00),
+ kY((byte) 0x02),
+ kZ((byte) 0x04);
+
+ /**
+ * The integer value representing this enumeration.
+ */
+ @SuppressWarnings("MemberName")
+ public final byte value;
+
+ Axes(byte value) {
+ this.value = value;
+ }
+ }
+
+ @SuppressWarnings("MemberName")
+ public static class AllAxes {
+ public double XAxis;
+ public double YAxis;
+ public double ZAxis;
+ }
+
+ protected SPI m_spi;
+
+ protected SimDevice m_simDevice;
+ protected SimEnum m_simRange;
+ protected SimDouble m_simX;
+ protected SimDouble m_simY;
+ protected SimDouble m_simZ;
+
+ /**
+ * Constructor.
+ *
+ * @param port The SPI port that the accelerometer is connected to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL345_SPI(SPI.Port port, Range range) {
+ m_spi = new SPI(port);
+ // simulation
+ m_simDevice = SimDevice.create("ADXL345_SPI", port.value);
+ if (m_simDevice != null) {
+ m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+ }
+ init(range);
+ SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_spi != null) {
+ m_spi.close();
+ m_spi = null;
+ }
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ m_simDevice = null;
+ }
+ }
+
+ /**
+ * Set SPI bus parameters, bring device out of sleep and set format.
+ *
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ private void init(Range range) {
+ m_spi.setClockRate(500000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnTrailingEdge();
+ m_spi.setClockActiveLow();
+ m_spi.setChipSelectActiveHigh();
+
+ // Turn on the measurements
+ byte[] commands = new byte[2];
+ commands[0] = kPowerCtlRegister;
+ commands[1] = kPowerCtl_Measure;
+ m_spi.write(commands, 2);
+
+ setRange(range);
+
+ HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_SPI);
+ }
+
+ @Override
+ public void setRange(Range range) {
+ final byte value;
+
+ switch (range) {
+ case k2G:
+ value = 0;
+ break;
+ case k4G:
+ value = 1;
+ break;
+ case k8G:
+ value = 2;
+ break;
+ case k16G:
+ value = 3;
+ break;
+ default:
+ throw new IllegalArgumentException(range + " unsupported");
+ }
+
+ // Specify the data format to read
+ byte[] commands = new byte[]{kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
+ m_spi.write(commands, commands.length);
+
+ if (m_simRange != null) {
+ m_simRange.set(value);
+ }
+ }
+
+ @Override
+ public double getX() {
+ return getAcceleration(Axes.kX);
+ }
+
+ @Override
+ public double getY() {
+ return getAcceleration(Axes.kY);
+ }
+
+ @Override
+ public double getZ() {
+ return getAcceleration(Axes.kZ);
+ }
+
+ /**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL345 in Gs.
+ */
+ public double getAcceleration(ADXL345_SPI.Axes axis) {
+ if (axis == Axes.kX && m_simX != null) {
+ return m_simX.get();
+ }
+ if (axis == Axes.kY && m_simY != null) {
+ return m_simY.get();
+ }
+ if (axis == Axes.kZ && m_simZ != null) {
+ return m_simZ.get();
+ }
+ ByteBuffer transferBuffer = ByteBuffer.allocate(3);
+ transferBuffer.put(0,
+ (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
+ m_spi.transaction(transferBuffer, transferBuffer, 3);
+ // Sensor is little endian
+ transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ return transferBuffer.getShort(1) * kGsPerLSB;
+ }
+
+ /**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
+ */
+ public ADXL345_SPI.AllAxes getAccelerations() {
+ ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
+ if (m_simX != null && m_simY != null && m_simZ != null) {
+ data.XAxis = m_simX.get();
+ data.YAxis = m_simY.get();
+ data.ZAxis = m_simZ.get();
+ return data;
+ }
+ if (m_spi != null) {
+ ByteBuffer dataBuffer = ByteBuffer.allocate(7);
+ // Select the data address.
+ dataBuffer.put(0, (byte) (kAddress_Read | kAddress_MultiByte | kDataRegister));
+ m_spi.transaction(dataBuffer, dataBuffer, 7);
+ // Sensor is little endian... swap bytes
+ dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ data.XAxis = dataBuffer.getShort(1) * kGsPerLSB;
+ data.YAxis = dataBuffer.getShort(3) * kGsPerLSB;
+ data.ZAxis = dataBuffer.getShort(5) * kGsPerLSB;
+ }
+ return data;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ NetworkTableEntry entryX = builder.getEntry("X");
+ NetworkTableEntry entryY = builder.getEntry("Y");
+ NetworkTableEntry entryZ = builder.getEntry("Z");
+ builder.setUpdateTable(() -> {
+ AllAxes data = getAccelerations();
+ entryX.setDouble(data.XAxis);
+ entryY.setDouble(data.YAxis);
+ entryZ.setDouble(data.ZAxis);
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
new file mode 100644
index 0000000..54583ea
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -0,0 +1,272 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * ADXL362 SPI Accelerometer.
+ *
+ * <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
+ */
+@SuppressWarnings("PMD.UnusedPrivateField")
+public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
+ private static final byte kRegWrite = 0x0A;
+ private static final byte kRegRead = 0x0B;
+
+ private static final byte kPartIdRegister = 0x02;
+ private static final byte kDataRegister = 0x0E;
+ private static final byte kFilterCtlRegister = 0x2C;
+ private static final byte kPowerCtlRegister = 0x2D;
+
+ private static final byte kFilterCtl_Range2G = 0x00;
+ private static final byte kFilterCtl_Range4G = 0x40;
+ private static final byte kFilterCtl_Range8G = (byte) 0x80;
+ private static final byte kFilterCtl_ODR_100Hz = 0x03;
+
+ private static final byte kPowerCtl_UltraLowNoise = 0x20;
+ private static final byte kPowerCtl_AutoSleep = 0x04;
+ private static final byte kPowerCtl_Measure = 0x02;
+
+ public enum Axes {
+ kX((byte) 0x00),
+ kY((byte) 0x02),
+ kZ((byte) 0x04);
+
+ @SuppressWarnings("MemberName")
+ public final byte value;
+
+ Axes(byte value) {
+ this.value = value;
+ }
+ }
+
+ @SuppressWarnings("MemberName")
+ public static class AllAxes {
+ public double XAxis;
+ public double YAxis;
+ public double ZAxis;
+ }
+
+ private SPI m_spi;
+
+ private SimDevice m_simDevice;
+ private SimEnum m_simRange;
+ private SimDouble m_simX;
+ private SimDouble m_simY;
+ private SimDouble m_simZ;
+
+ private double m_gsPerLSB;
+
+ /**
+ * Constructor. Uses the onboard CS1.
+ *
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL362(Range range) {
+ this(SPI.Port.kOnboardCS1, range);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param port The SPI port that the accelerometer is connected to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL362(SPI.Port port, Range range) {
+ m_spi = new SPI(port);
+
+ // simulation
+ m_simDevice = SimDevice.create("ADXL362", port.value);
+ if (m_simDevice != null) {
+ m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+ }
+
+ m_spi.setClockRate(3000000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnTrailingEdge();
+ m_spi.setClockActiveLow();
+ m_spi.setChipSelectActiveLow();
+
+ ByteBuffer transferBuffer = ByteBuffer.allocate(3);
+ if (m_simDevice == null) {
+ // Validate the part ID
+ transferBuffer.put(0, kRegRead);
+ transferBuffer.put(1, kPartIdRegister);
+ m_spi.transaction(transferBuffer, transferBuffer, 3);
+ if (transferBuffer.get(2) != (byte) 0xF2) {
+ m_spi.close();
+ m_spi = null;
+ DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
+ return;
+ }
+ }
+
+ setRange(range);
+
+ // Turn on the measurements
+ transferBuffer.put(0, kRegWrite);
+ transferBuffer.put(1, kPowerCtlRegister);
+ transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise));
+ m_spi.write(transferBuffer, 3);
+
+ HAL.report(tResourceType.kResourceType_ADXL362, port.value + 1);
+ SendableRegistry.addLW(this, "ADXL362", port.value);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_spi != null) {
+ m_spi.close();
+ m_spi = null;
+ }
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ m_simDevice = null;
+ }
+ }
+
+ @Override
+ public void setRange(Range range) {
+ if (m_spi == null) {
+ return;
+ }
+
+ final byte value;
+ switch (range) {
+ case k2G:
+ value = kFilterCtl_Range2G;
+ m_gsPerLSB = 0.001;
+ break;
+ case k4G:
+ value = kFilterCtl_Range4G;
+ m_gsPerLSB = 0.002;
+ break;
+ case k8G:
+ case k16G: // 16G not supported; treat as 8G
+ value = kFilterCtl_Range8G;
+ m_gsPerLSB = 0.004;
+ break;
+ default:
+ throw new IllegalArgumentException(range + " unsupported");
+
+ }
+
+ // Specify the data format to read
+ byte[] commands = new byte[]{kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz
+ | value)};
+ m_spi.write(commands, commands.length);
+
+ if (m_simRange != null) {
+ m_simRange.set(value);
+ }
+ }
+
+
+ @Override
+ public double getX() {
+ return getAcceleration(Axes.kX);
+ }
+
+ @Override
+ public double getY() {
+ return getAcceleration(Axes.kY);
+ }
+
+ @Override
+ public double getZ() {
+ return getAcceleration(Axes.kZ);
+ }
+
+ /**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL362 in Gs.
+ */
+ public double getAcceleration(ADXL362.Axes axis) {
+ if (axis == Axes.kX && m_simX != null) {
+ return m_simX.get();
+ }
+ if (axis == Axes.kY && m_simY != null) {
+ return m_simY.get();
+ }
+ if (axis == Axes.kZ && m_simZ != null) {
+ return m_simZ.get();
+ }
+ if (m_spi == null) {
+ return 0.0;
+ }
+ ByteBuffer transferBuffer = ByteBuffer.allocate(4);
+ transferBuffer.put(0, kRegRead);
+ transferBuffer.put(1, (byte) (kDataRegister + axis.value));
+ m_spi.transaction(transferBuffer, transferBuffer, 4);
+ // Sensor is little endian
+ transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ return transferBuffer.getShort(2) * m_gsPerLSB;
+ }
+
+ /**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the ADXL362 in Gs.
+ */
+ public ADXL362.AllAxes getAccelerations() {
+ ADXL362.AllAxes data = new ADXL362.AllAxes();
+ if (m_simX != null && m_simY != null && m_simZ != null) {
+ data.XAxis = m_simX.get();
+ data.YAxis = m_simY.get();
+ data.ZAxis = m_simZ.get();
+ return data;
+ }
+ if (m_spi != null) {
+ ByteBuffer dataBuffer = ByteBuffer.allocate(8);
+ // Select the data address.
+ dataBuffer.put(0, kRegRead);
+ dataBuffer.put(1, kDataRegister);
+ m_spi.transaction(dataBuffer, dataBuffer, 8);
+ // Sensor is little endian... swap bytes
+ dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ data.XAxis = dataBuffer.getShort(2) * m_gsPerLSB;
+ data.YAxis = dataBuffer.getShort(4) * m_gsPerLSB;
+ data.ZAxis = dataBuffer.getShort(6) * m_gsPerLSB;
+ }
+ return data;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ NetworkTableEntry entryX = builder.getEntry("X");
+ NetworkTableEntry entryY = builder.getEntry("Y");
+ NetworkTableEntry entryZ = builder.getEntry("Z");
+ builder.setUpdateTable(() -> {
+ AllAxes data = getAccelerations();
+ entryX.setDouble(data.XAxis);
+ entryY.setDouble(data.YAxis);
+ entryZ.setDouble(data.ZAxis);
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
new file mode 100644
index 0000000..549a91a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -0,0 +1,210 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
+ * tracks the robots heading based on the starting position. As the robot rotates the new heading is
+ * computed by integrating the rate of rotation returned by the sensor. When the class is
+ * instantiated, it does a short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to determine the heading.
+ *
+ * <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
+public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable, AutoCloseable {
+ private static final double kSamplePeriod = 0.0005;
+ private static final double kCalibrationSampleTime = 5.0;
+ private static final double kDegreePerSecondPerLSB = 0.0125;
+
+ private static final int kRateRegister = 0x00;
+ private static final int kTemRegister = 0x02;
+ private static final int kLoCSTRegister = 0x04;
+ private static final int kHiCSTRegister = 0x06;
+ private static final int kQuadRegister = 0x08;
+ private static final int kFaultRegister = 0x0A;
+ private static final int kPIDRegister = 0x0C;
+ private static final int kSNHighRegister = 0x0E;
+ private static final int kSNLowRegister = 0x10;
+
+ private SPI m_spi;
+
+ private SimDevice m_simDevice;
+ private SimBoolean m_simConnected;
+ private SimDouble m_simAngle;
+ private SimDouble m_simRate;
+
+ /**
+ * Constructor. Uses the onboard CS0.
+ */
+ public ADXRS450_Gyro() {
+ this(SPI.Port.kOnboardCS0);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param port The SPI port that the gyro is connected to
+ */
+ public ADXRS450_Gyro(SPI.Port port) {
+ m_spi = new SPI(port);
+
+ // simulation
+ m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
+ if (m_simDevice != null) {
+ m_simConnected = m_simDevice.createBoolean("Connected", false, true);
+ m_simAngle = m_simDevice.createDouble("Angle", false, 0.0);
+ m_simRate = m_simDevice.createDouble("Rate", false, 0.0);
+ }
+
+ m_spi.setClockRate(3000000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnLeadingEdge();
+ m_spi.setClockActiveHigh();
+ m_spi.setChipSelectActiveLow();
+
+ if (m_simDevice == null) {
+ // Validate the part ID
+ if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
+ m_spi.close();
+ m_spi = null;
+ DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
+ false);
+ return;
+ }
+
+ m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
+ true, true);
+
+ calibrate();
+ }
+
+ HAL.report(tResourceType.kResourceType_ADXRS450, port.value + 1);
+ SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
+ }
+
+ /**
+ * Determine if the gyro is connected.
+ *
+ * @return true if it is connected, false otherwise.
+ */
+ public boolean isConnected() {
+ if (m_simConnected != null) {
+ return m_simConnected.get();
+ }
+ return m_spi != null;
+ }
+
+ @Override
+ public void calibrate() {
+ if (m_spi == null) {
+ return;
+ }
+
+ Timer.delay(0.1);
+
+ m_spi.setAccumulatorIntegratedCenter(0);
+ m_spi.resetAccumulator();
+
+ Timer.delay(kCalibrationSampleTime);
+
+ m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage());
+ m_spi.resetAccumulator();
+ }
+
+ private boolean calcParity(int value) {
+ boolean parity = false;
+ while (value != 0) {
+ parity = !parity;
+ value = value & (value - 1);
+ }
+ return parity;
+ }
+
+ private int readRegister(int reg) {
+ int cmdhi = 0x8000 | (reg << 1);
+ boolean parity = calcParity(cmdhi);
+
+ ByteBuffer buf = ByteBuffer.allocate(4);
+ buf.order(ByteOrder.BIG_ENDIAN);
+ buf.put(0, (byte) (cmdhi >> 8));
+ buf.put(1, (byte) (cmdhi & 0xff));
+ buf.put(2, (byte) 0);
+ buf.put(3, (byte) (parity ? 0 : 1));
+
+ m_spi.write(buf, 4);
+ m_spi.read(false, buf, 4);
+
+ if ((buf.get(0) & 0xe0) == 0) {
+ return 0; // error, return 0
+ }
+ return (buf.getInt(0) >> 5) & 0xffff;
+ }
+
+ @Override
+ public void reset() {
+ if (m_simAngle != null) {
+ m_simAngle.set(0.0);
+ }
+ if (m_simRate != null) {
+ m_simRate.set(0.0);
+ }
+ if (m_spi != null) {
+ m_spi.resetAccumulator();
+ }
+ }
+
+ /**
+ * Delete (free) the spi port used for the gyro and stop accumulating.
+ */
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_spi != null) {
+ m_spi.close();
+ m_spi = null;
+ }
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ m_simDevice = null;
+ }
+ }
+
+ @Override
+ public double getAngle() {
+ if (m_simAngle != null) {
+ return m_simAngle.get();
+ }
+ if (m_spi == null) {
+ return 0.0;
+ }
+ return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
+ }
+
+ @Override
+ public double getRate() {
+ if (m_simRate != null) {
+ return m_simRate.get();
+ }
+ if (m_spi == null) {
+ return 0.0;
+ }
+ return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
new file mode 100644
index 0000000..223c6f2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AddressableLEDJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PWMJNI;
+
+/**
+ * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ */
+public class AddressableLED implements AutoCloseable {
+ private final int m_pwmHandle;
+ private final int m_handle;
+
+ /**
+ * Constructs a new driver for a specific port.
+ *
+ * @param port the output port to use (Must be a PWM port)
+ */
+ public AddressableLED(int port) {
+ m_pwmHandle = PWMJNI.initializePWMPort(HAL.getPort((byte) port));
+ m_handle = AddressableLEDJNI.initialize(m_pwmHandle);
+ HAL.report(tResourceType.kResourceType_AddressableLEDs, port + 1);
+ }
+
+ @Override
+ public void close() {
+ if (m_handle != 0) {
+ AddressableLEDJNI.free(m_handle);
+ }
+ if (m_pwmHandle != 0) {
+ PWMJNI.freePWMPort(m_pwmHandle);
+ }
+ }
+
+ /**
+ * Sets the length of the LED strip.
+ *
+ * <p>Calling this is an expensive call, so its best to call it once, then just update data.
+ *
+ * @param length the strip length
+ */
+ public void setLength(int length) {
+ AddressableLEDJNI.setLength(m_handle, length);
+ }
+
+ /**
+ * Sets the led output data.
+ *
+ * <p>If the output is enabled, this will start writing the next data cycle.
+ * It is safe to call, even while output is enabled.
+ *
+ * @param buffer the buffer to write
+ */
+ public void setData(AddressableLEDBuffer buffer) {
+ AddressableLEDJNI.setData(m_handle, buffer.m_buffer);
+ }
+
+ /**
+ * Sets the bit timing.
+ *
+ * <p>By default, the driver is set up to drive WS2812s, so nothing needs to be set for those.
+ *
+ * @param lowTime0NanoSeconds low time for 0 bit
+ * @param highTime0NanoSeconds high time for 0 bit
+ * @param lowTime1NanoSeconds low time for 1 bit
+ * @param highTime1NanoSeconds high time for 1 bit
+ */
+ public void setBitTiming(int lowTime0NanoSeconds, int highTime0NanoSeconds,
+ int lowTime1NanoSeconds, int highTime1NanoSeconds) {
+ AddressableLEDJNI.setBitTiming(m_handle, lowTime0NanoSeconds,
+ highTime0NanoSeconds, lowTime1NanoSeconds,
+ highTime1NanoSeconds);
+ }
+
+ /**
+ * Sets the sync time.
+ *
+ * <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812.
+ *
+ * @param syncTimeMicroSeconds the sync time
+ */
+ public void setSyncTime(int syncTimeMicroSeconds) {
+ AddressableLEDJNI.setSyncTime(m_handle, syncTimeMicroSeconds);
+ }
+
+ /**
+ * Starts the output.
+ *
+ * <p>The output writes continously.
+ */
+ public void start() {
+ AddressableLEDJNI.start(m_handle);
+ }
+
+ /**
+ * Stops the output.
+ */
+ public void stop() {
+ AddressableLEDJNI.stop(m_handle);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
new file mode 100644
index 0000000..3cc0763
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * Buffer storage for Addressable LEDs.
+ */
+public class AddressableLEDBuffer {
+ byte[] m_buffer;
+
+ /**
+ * Constructs a new LED buffer with the specified length.
+ *
+ * @param length The length of the buffer in pixels
+ */
+ public AddressableLEDBuffer(int length) {
+ m_buffer = new byte[length * 4];
+ }
+
+ /**
+ * Sets a specific led in the buffer.
+ *
+ * @param index the index to write
+ * @param r the r value [0-255]
+ * @param g the g value [0-255]
+ * @param b the b value [0-255]
+ */
+ @SuppressWarnings("ParameterName")
+ public void setRGB(int index, int r, int g, int b) {
+ m_buffer[index * 4] = (byte) b;
+ m_buffer[(index * 4) + 1] = (byte) g;
+ m_buffer[(index * 4) + 2] = (byte) r;
+ m_buffer[(index * 4) + 3] = 0;
+ }
+
+ /**
+ * Sets a specific led in the buffer.
+ *
+ * @param index the index to write
+ * @param h the h value [0-180]
+ * @param s the s value [0-255]
+ * @param v the v value [0-255]
+ */
+ @SuppressWarnings("ParameterName")
+ public void setHSV(final int index, final int h, final int s, final int v) {
+ if (s == 0) {
+ setRGB(index, v, v, v);
+ return;
+ }
+
+ final int region = h / 30;
+ final int remainder = (h - (region * 30)) * 6;
+
+ final int p = (v * (255 - s)) >> 8;
+ final int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+ final int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+ switch (region) {
+ case 0:
+ setRGB(index, v, t, p);
+ break;
+ case 1:
+ setRGB(index, q, v, p);
+ break;
+ case 2:
+ setRGB(index, p, v, t);
+ break;
+ case 3:
+ setRGB(index, p, q, v);
+ break;
+ case 4:
+ setRGB(index, t, p, v);
+ break;
+ default:
+ setRGB(index, v, p, q);
+ break;
+ }
+ }
+
+ /**
+ * Sets a specific LED in the buffer.
+ *
+ * @param index The index to write
+ * @param color The color of the LED
+ */
+ public void setLED(int index, Color color) {
+ setRGB(index,
+ (int) (color.red * 255),
+ (int) (color.green * 255),
+ (int) (color.blue * 255));
+ }
+
+ /**
+ * Sets a specific LED in the buffer.
+ *
+ * @param index The index to write
+ * @param color The color of the LED
+ */
+ public void setLED(int index, Color8Bit color) {
+ setRGB(index, color.red, color.green, color.blue);
+ }
+
+ /**
+ * Gets the buffer length.
+ *
+ * @return the buffer length
+ */
+ public int getLength() {
+ return m_buffer.length / 4;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
new file mode 100644
index 0000000..41c5b38
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * Handle operation of an analog accelerometer. The accelerometer reads acceleration directly
+ * through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each
+ * is calibrated by finding the center value over a period of time.
+ */
+public class AnalogAccelerometer implements PIDSource, Sendable, AutoCloseable {
+ private AnalogInput m_analogChannel;
+ private double m_voltsPerG = 1.0;
+ private double m_zeroGVoltage = 2.5;
+ private final boolean m_allocatedChannel;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Common initialization.
+ */
+ private void initAccelerometer() {
+ HAL.report(tResourceType.kResourceType_Accelerometer,
+ m_analogChannel.getChannel() + 1);
+ SendableRegistry.addLW(this, "Accelerometer", m_analogChannel.getChannel());
+ }
+
+ /**
+ * Create a new instance of an accelerometer.
+ *
+ * <p>The constructor allocates desired analog channel.
+ *
+ * @param channel The channel number for the analog input the accelerometer is connected to
+ */
+ public AnalogAccelerometer(final int channel) {
+ this(new AnalogInput(channel), true);
+ SendableRegistry.addChild(this, m_analogChannel);
+ }
+
+ /**
+ * Create a new instance of Accelerometer from an existing AnalogChannel. Make a new instance of
+ * accelerometer given an AnalogChannel. This is particularly useful if the port is going to be
+ * read as an analog channel as well as through the Accelerometer class.
+ *
+ * @param channel The existing AnalogInput object for the analog input the accelerometer is
+ * connected to
+ */
+ public AnalogAccelerometer(final AnalogInput channel) {
+ this(channel, false);
+ }
+
+ private AnalogAccelerometer(final AnalogInput channel, final boolean allocatedChannel) {
+ requireNonNullParam(channel, "channel", "AnalogAccelerometer");
+ m_allocatedChannel = allocatedChannel;
+ m_analogChannel = channel;
+ initAccelerometer();
+ }
+
+ /**
+ * Delete the analog components used for the accelerometer.
+ */
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_analogChannel != null && m_allocatedChannel) {
+ m_analogChannel.close();
+ }
+ m_analogChannel = null;
+ }
+
+ /**
+ * Return the acceleration in Gs.
+ *
+ * <p>The acceleration is returned units of Gs.
+ *
+ * @return The current acceleration of the sensor in Gs.
+ */
+ public double getAcceleration() {
+ if (m_analogChannel == null) {
+ return 0.0;
+ }
+ return (m_analogChannel.getAverageVoltage() - m_zeroGVoltage) / m_voltsPerG;
+ }
+
+ /**
+ * Set the accelerometer sensitivity.
+ *
+ * <p>This sets the sensitivity of the accelerometer used for calculating the acceleration. The
+ * sensitivity varies by accelerometer model. There are constants defined for various models.
+ *
+ * @param sensitivity The sensitivity of accelerometer in Volts per G.
+ */
+ public void setSensitivity(double sensitivity) {
+ m_voltsPerG = sensitivity;
+ }
+
+ /**
+ * Set the voltage that corresponds to 0 G.
+ *
+ * <p>The zero G voltage varies by accelerometer model. There are constants defined for various
+ * models.
+ *
+ * @param zero The zero G voltage.
+ */
+ public void setZero(double zero) {
+ m_zeroGVoltage = zero;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the Acceleration for the PID Source parent.
+ *
+ * @return The current acceleration in Gs.
+ */
+ @Override
+ public double pidGet() {
+ return getAcceleration();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Accelerometer");
+ builder.addDoubleProperty("Value", this::getAcceleration, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
new file mode 100644
index 0000000..75177e0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for supporting continuous analog encoders, such as the US Digital MA3.
+ */
+public class AnalogEncoder implements Sendable, AutoCloseable {
+ private final AnalogInput m_analogInput;
+ private AnalogTrigger m_analogTrigger;
+ private Counter m_counter;
+ private double m_positionOffset;
+ private double m_distancePerRotation = 1.0;
+ private double m_lastPosition;
+
+ protected SimDevice m_simDevice;
+ protected SimDouble m_simPosition;
+
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ public AnalogEncoder(AnalogInput analogInput) {
+ m_analogInput = analogInput;
+ init();
+ }
+
+ private void init() {
+ m_analogTrigger = new AnalogTrigger(m_analogInput);
+ m_counter = new Counter();
+
+ m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
+
+ if (m_simDevice != null) {
+ m_simPosition = m_simDevice.createDouble("Position", false, 0.0);
+ }
+
+ // Limits need to be 25% from each end
+ m_analogTrigger.setLimitsVoltage(1.25, 3.75);
+ m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
+ m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
+
+ SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel());
+ }
+
+ /**
+ * Get the encoder value since the last reset.
+ *
+ * <p>This is reported in rotations since the last reset.
+ *
+ * @return the encoder value in rotations
+ */
+ public double get() {
+ if (m_simPosition != null) {
+ return m_simPosition.get();
+ }
+
+ // As the values are not atomic, keep trying until we get 2 reads of the same
+ // value. If we don't within 10 attempts, warn
+ for (int i = 0; i < 10; i++) {
+ double counter = m_counter.get();
+ double pos = m_analogInput.getVoltage();
+ double counter2 = m_counter.get();
+ double pos2 = m_analogInput.getVoltage();
+ if (counter == counter2 && pos == pos2) {
+ double position = counter + pos - m_positionOffset;
+ m_lastPosition = position;
+ return position;
+ }
+ }
+
+ DriverStation.reportWarning(
+ "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
+ return m_lastPosition;
+ }
+
+ /**
+ * Get the offset of position relative to the last reset.
+ *
+ * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute
+ * position relative to the last reset. This could potentially be negative,
+ * which needs to be accounted for.
+ *
+ * @return the position offset
+ */
+ public double getPositionOffset() {
+ return m_positionOffset;
+ }
+
+ /**
+ * Set the distance per rotation of the encoder. This sets the multiplier used
+ * to determine the distance driven based on the rotation value from the
+ * encoder. Set this value based on the how far the mechanism travels in 1
+ * rotation of the encoder, and factor in gearing reductions following the
+ * encoder shaft. This distance can be in any units you like, linear or angular.
+ *
+ * @param distancePerRotation the distance per rotation of the encoder
+ */
+ public void setDistancePerRotation(double distancePerRotation) {
+ m_distancePerRotation = distancePerRotation;
+ }
+
+ /**
+ * Get the distance per rotation for this encoder.
+ *
+ * @return The scale factor that will be used to convert rotation to useful
+ * units.
+ */
+ public double getDistancePerRotation() {
+ return m_distancePerRotation;
+ }
+
+ /**
+ * Get the distance the sensor has driven since the last reset as scaled by the
+ * value from {@link #setDistancePerRotation(double)}.
+ *
+ * @return The distance driven since the last reset
+ */
+ public double getDistance() {
+ return get() * getDistancePerRotation();
+ }
+
+ /**
+ * Reset the Encoder distance to zero.
+ */
+ public void reset() {
+ m_counter.reset();
+ m_positionOffset = m_analogInput.getVoltage();
+ }
+
+ @Override
+ public void close() {
+ m_counter.close();
+ m_analogTrigger.close();
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("AbsoluteEncoder");
+ builder.addDoubleProperty("Distance", this::getDistance, null);
+ builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
new file mode 100644
index 0000000..c688b55
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogGyroJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
+ * tracks the robots heading based on the starting position. As the robot rotates the new heading is
+ * computed by integrating the rate of rotation returned by the sensor. When the class is
+ * instantiated, it does a short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to determine the heading.
+ *
+ * <p>This class is for gyro sensors that connect to an analog input.
+ */
+public class AnalogGyro extends GyroBase implements Gyro, PIDSource, Sendable, AutoCloseable {
+ private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
+ protected AnalogInput m_analog;
+ private boolean m_channelAllocated;
+
+ private int m_gyroHandle;
+
+ /**
+ * Initialize the gyro. Calibration is handled by calibrate().
+ */
+ public void initGyro() {
+ if (m_gyroHandle == 0) {
+ m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
+ }
+
+ AnalogGyroJNI.setupAnalogGyro(m_gyroHandle);
+
+ HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel() + 1);
+ SendableRegistry.addLW(this, "AnalogGyro", m_analog.getChannel());
+ }
+
+ @Override
+ public void calibrate() {
+ AnalogGyroJNI.calibrateAnalogGyro(m_gyroHandle);
+ }
+
+ /**
+ * Gyro constructor using the channel number.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
+ * channels 0-1.
+ */
+ public AnalogGyro(int channel) {
+ this(new AnalogInput(channel));
+ m_channelAllocated = true;
+ SendableRegistry.addChild(this, m_analog);
+ }
+
+ /**
+ * Gyro constructor with a precreated analog channel object. Use this constructor when the analog
+ * channel needs to be shared.
+ *
+ * @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
+ * on-board channels 0-1.
+ */
+ public AnalogGyro(AnalogInput channel) {
+ requireNonNullParam(channel, "channel", "AnalogGyro");
+
+ m_analog = channel;
+ initGyro();
+ calibrate();
+ }
+
+ /**
+ * Gyro constructor using the channel number along with parameters for presetting the center and
+ * offset values. Bypasses calibration.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
+ * channels 0-1.
+ * @param center Preset uncalibrated value to use as the accumulator center value.
+ * @param offset Preset uncalibrated value to use as the gyro offset.
+ */
+ public AnalogGyro(int channel, int center, double offset) {
+ this(new AnalogInput(channel), center, offset);
+ m_channelAllocated = true;
+ SendableRegistry.addChild(this, m_analog);
+ }
+
+ /**
+ * Gyro constructor with a precreated analog channel object along with parameters for presetting
+ * the center and offset values. Bypasses calibration.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
+ * channels 0-1.
+ * @param center Preset uncalibrated value to use as the accumulator center value.
+ * @param offset Preset uncalibrated value to use as the gyro offset.
+ */
+ public AnalogGyro(AnalogInput channel, int center, double offset) {
+ requireNonNullParam(channel, "channel", "AnalogGyro");
+
+ m_analog = channel;
+ initGyro();
+ AnalogGyroJNI.setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
+ offset, center);
+ reset();
+ }
+
+ @Override
+ public void reset() {
+ AnalogGyroJNI.resetAnalogGyro(m_gyroHandle);
+ }
+
+ /**
+ * Delete (free) the accumulator and the analog components used for the gyro.
+ */
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_analog != null && m_channelAllocated) {
+ m_analog.close();
+ }
+ m_analog = null;
+ AnalogGyroJNI.freeAnalogGyro(m_gyroHandle);
+ }
+
+ @Override
+ public double getAngle() {
+ if (m_analog == null) {
+ return 0.0;
+ } else {
+ return AnalogGyroJNI.getAnalogGyroAngle(m_gyroHandle);
+ }
+ }
+
+ @Override
+ public double getRate() {
+ if (m_analog == null) {
+ return 0.0;
+ } else {
+ return AnalogGyroJNI.getAnalogGyroRate(m_gyroHandle);
+ }
+ }
+
+ /**
+ * Return the gyro offset value set during calibration to use as a future preset.
+ *
+ * @return the current offset value
+ */
+ public double getOffset() {
+ return AnalogGyroJNI.getAnalogGyroOffset(m_gyroHandle);
+ }
+
+ /**
+ * Return the gyro center value set during calibration to use as a future preset.
+ *
+ * @return the current center value
+ */
+ public int getCenter() {
+ return AnalogGyroJNI.getAnalogGyroCenter(m_gyroHandle);
+ }
+
+ /**
+ * Set the gyro sensitivity. This takes the number of volts/degree/second sensitivity of the gyro
+ * and uses it in subsequent calculations to allow the code to work with multiple gyros. This
+ * value is typically found in the gyro datasheet.
+ *
+ * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
+ */
+ public void setSensitivity(double voltsPerDegreePerSecond) {
+ AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
+ voltsPerDegreePerSecond);
+ }
+
+ /**
+ * Set the size of the neutral zone. Any voltage from the gyro less than this amount from the
+ * center is considered stationary. Setting a deadband will decrease the amount of drift when the
+ * gyro isn't rotating, but will make it less accurate.
+ *
+ * @param volts The size of the deadband in volts
+ */
+ void setDeadband(double volts) {
+ AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, volts);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
new file mode 100644
index 0000000..308be6b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -0,0 +1,367 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AccumulatorResult;
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.sim.AnalogInSim;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Analog channel class.
+ *
+ * <p>Each analog channel is read from hardware as a 12-bit number representing 0V to 5V.
+ *
+ * <p>Connected to each analog channel is an averaging and oversampling engine. This engine
+ * accumulates the specified ( by setAverageBits() and setOversampleBits() ) number of samples
+ * before returning a new value. This is not a sliding window average. The only difference between
+ * the oversampled samples and the averaged samples is that the oversampled samples are simply
+ * accumulated effectively increasing the resolution, while the averaged samples are divided by the
+ * number of samples to retain the resolution, but get more stable values.
+ */
+public class AnalogInput implements PIDSource, Sendable, AutoCloseable {
+ private static final int kAccumulatorSlot = 1;
+ int m_port; // explicit no modifier, private and package accessible.
+ private int m_channel;
+ private static final int[] kAccumulatorChannels = {0, 1};
+ private long m_accumulatorOffset;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Construct an analog channel.
+ *
+ * @param channel The channel number to represent. 0-3 are on-board 4-7 are on the MXP port.
+ */
+ public AnalogInput(final int channel) {
+ AnalogJNI.checkAnalogInputChannel(channel);
+ m_channel = channel;
+
+ final int portHandle = HAL.getPort((byte) channel);
+ m_port = AnalogJNI.initializeAnalogInputPort(portHandle);
+
+ HAL.report(tResourceType.kResourceType_AnalogChannel, channel + 1);
+ SendableRegistry.addLW(this, "AnalogInput", channel);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ AnalogJNI.freeAnalogInputPort(m_port);
+ m_port = 0;
+ m_channel = 0;
+ m_accumulatorOffset = 0;
+ }
+
+ /**
+ * Get a sample straight from this channel. The sample is a 12-bit value representing the 0V to 5V
+ * range of the A/D converter. The units are in A/D converter codes. Use GetVoltage() to get the
+ * analog value in calibrated units.
+ *
+ * @return A sample straight from this channel.
+ */
+ public int getValue() {
+ return AnalogJNI.getAnalogValue(m_port);
+ }
+
+ /**
+ * Get a sample from the output of the oversample and average engine for this channel. The sample
+ * is 12-bit + the bits configured in SetOversampleBits(). The value configured in
+ * setAverageBits() will cause this value to be averaged 2^bits number of samples. This is not a
+ * sliding window. The sample will not change until 2^(OversampleBits + AverageBits) samples have
+ * been acquired from this channel. Use getAverageVoltage() to get the analog value in calibrated
+ * units.
+ *
+ * @return A sample from the oversample and average engine for this channel.
+ */
+ public int getAverageValue() {
+ return AnalogJNI.getAnalogAverageValue(m_port);
+ }
+
+ /**
+ * Get a scaled sample straight from this channel. The value is scaled to units of Volts using the
+ * calibrated scaling data from getLSBWeight() and getOffset().
+ *
+ * @return A scaled sample straight from this channel.
+ */
+ public double getVoltage() {
+ return AnalogJNI.getAnalogVoltage(m_port);
+ }
+
+ /**
+ * Get a scaled sample from the output of the oversample and average engine for this channel. The
+ * value is scaled to units of Volts using the calibrated scaling data from getLSBWeight() and
+ * getOffset(). Using oversampling will cause this value to be higher resolution, but it will
+ * update more slowly. Using averaging will cause this value to be more stable, but it will update
+ * more slowly.
+ *
+ * @return A scaled sample from the output of the oversample and average engine for this channel.
+ */
+ public double getAverageVoltage() {
+ return AnalogJNI.getAnalogAverageVoltage(m_port);
+ }
+
+ /**
+ * Get the factory scaling least significant bit weight constant. The least significant bit weight
+ * constant for the channel that was calibrated in manufacturing and stored in an eeprom.
+ *
+ * <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @return Least significant bit weight.
+ */
+ public long getLSBWeight() {
+ return AnalogJNI.getAnalogLSBWeight(m_port);
+ }
+
+ /**
+ * Get the factory scaling offset constant. The offset constant for the channel that was
+ * calibrated in manufacturing and stored in an eeprom.
+ *
+ * <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @return Offset constant.
+ */
+ public int getOffset() {
+ return AnalogJNI.getAnalogOffset(m_port);
+ }
+
+ /**
+ * Get the channel number.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Set the number of averaging bits. This sets the number of averaging bits. The actual number of
+ * averaged samples is 2^bits. The averaging is done automatically in the FPGA.
+ *
+ * @param bits The number of averaging bits.
+ */
+ public void setAverageBits(final int bits) {
+ AnalogJNI.setAnalogAverageBits(m_port, bits);
+ }
+
+ /**
+ * Get the number of averaging bits. This gets the number of averaging bits from the FPGA. The
+ * actual number of averaged samples is 2^bits. The averaging is done automatically in the FPGA.
+ *
+ * @return The number of averaging bits.
+ */
+ public int getAverageBits() {
+ return AnalogJNI.getAnalogAverageBits(m_port);
+ }
+
+ /**
+ * Set the number of oversample bits. This sets the number of oversample bits. The actual number
+ * of oversampled values is 2^bits. The oversampling is done automatically in the FPGA.
+ *
+ * @param bits The number of oversample bits.
+ */
+ public void setOversampleBits(final int bits) {
+ AnalogJNI.setAnalogOversampleBits(m_port, bits);
+ }
+
+ /**
+ * Get the number of oversample bits. This gets the number of oversample bits from the FPGA. The
+ * actual number of oversampled values is 2^bits. The oversampling is done automatically in the
+ * FPGA.
+ *
+ * @return The number of oversample bits.
+ */
+ public int getOversampleBits() {
+ return AnalogJNI.getAnalogOversampleBits(m_port);
+ }
+
+ /**
+ * Initialize the accumulator.
+ */
+ public void initAccumulator() {
+ if (!isAccumulatorChannel()) {
+ throw new AllocationException("Accumulators are only available on slot " + kAccumulatorSlot
+ + " on channels " + kAccumulatorChannels[0] + ", " + kAccumulatorChannels[1]);
+ }
+ m_accumulatorOffset = 0;
+ AnalogJNI.initAccumulator(m_port);
+ }
+
+ /**
+ * Set an initial value for the accumulator.
+ *
+ * <p>This will be added to all values returned to the user.
+ *
+ * @param initialValue The value that the accumulator should start from when reset.
+ */
+ public void setAccumulatorInitialValue(long initialValue) {
+ m_accumulatorOffset = initialValue;
+ }
+
+ /**
+ * Resets the accumulator to the initial value.
+ */
+ public void resetAccumulator() {
+ AnalogJNI.resetAccumulator(m_port);
+
+ // Wait until the next sample, so the next call to getAccumulator*()
+ // won't have old values.
+ final double sampleTime = 1.0 / getGlobalSampleRate();
+ final double overSamples = 1 << getOversampleBits();
+ final double averageSamples = 1 << getAverageBits();
+ Timer.delay(sampleTime * overSamples * averageSamples);
+
+ }
+
+ /**
+ * Set the center value of the accumulator.
+ *
+ * <p>The center value is subtracted from each A/D value before it is added to the accumulator.
+ * This is used for the center value of devices like gyros and accelerometers to take the device
+ * offset into account when integrating.
+ *
+ * <p>This center value is based on the output of the oversampled and averaged source the
+ * accumulator channel. Because of this, any non-zero oversample bits will affect the size of the
+ * value for this field.
+ */
+ public void setAccumulatorCenter(int center) {
+ AnalogJNI.setAccumulatorCenter(m_port, center);
+ }
+
+ /**
+ * Set the accumulator's deadband.
+ *
+ * @param deadband The deadband size in ADC codes (12-bit value)
+ */
+ public void setAccumulatorDeadband(int deadband) {
+ AnalogJNI.setAccumulatorDeadband(m_port, deadband);
+ }
+
+ /**
+ * Read the accumulated value.
+ *
+ * <p>Read the value that has been accumulating. The accumulator is attached after the oversample
+ * and average engine.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+ public long getAccumulatorValue() {
+ return AnalogJNI.getAccumulatorValue(m_port) + m_accumulatorOffset;
+ }
+
+ /**
+ * Read the number of accumulated values.
+ *
+ * <p>Read the count of the accumulated values since the accumulator was last Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+ public long getAccumulatorCount() {
+ return AnalogJNI.getAccumulatorCount(m_port);
+ }
+
+ /**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * <p>This function reads the value and count from the FPGA atomically. This can be used for
+ * averaging.
+ *
+ * @param result AccumulatorResult object to store the results in.
+ */
+ public void getAccumulatorOutput(AccumulatorResult result) {
+ if (result == null) {
+ throw new IllegalArgumentException("Null parameter `result'");
+ }
+ if (!isAccumulatorChannel()) {
+ throw new IllegalArgumentException(
+ "Channel " + m_channel + " is not an accumulator channel.");
+ }
+ AnalogJNI.getAccumulatorOutput(m_port, result);
+ result.value += m_accumulatorOffset;
+ }
+
+ /**
+ * Is the channel attached to an accumulator.
+ *
+ * @return The analog channel is attached to an accumulator.
+ */
+ public boolean isAccumulatorChannel() {
+ for (int channel : kAccumulatorChannels) {
+ if (m_channel == channel) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Set the sample rate per channel.
+ *
+ * <p>This is a global setting for all channels. The maximum rate is 500kS/s divided by the number
+ * of channels in use. This is 62500 samples/s per channel if all 8 channels are used.
+ *
+ * @param samplesPerSecond The number of samples per second.
+ */
+ public static void setGlobalSampleRate(final double samplesPerSecond) {
+ AnalogJNI.setAnalogSampleRate(samplesPerSecond);
+ }
+
+ /**
+ * Get the current sample rate.
+ *
+ * <p>This assumes one entry in the scan list. This is a global setting for all channels.
+ *
+ * @return Sample rate.
+ */
+ public static double getGlobalSampleRate() {
+ return AnalogJNI.getAnalogSampleRate();
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the average voltage for use with PIDController.
+ *
+ * @return the average voltage
+ */
+ @Override
+ public double pidGet() {
+ return getAverageVoltage();
+ }
+
+ /**
+ * Indicates this input is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ public void setSimDevice(SimDevice device) {
+ AnalogJNI.setAnalogInputSimDevice(m_port, device.getNativeHandle());
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Analog Input");
+ builder.addDoubleProperty("Value", this::getAverageVoltage, null);
+ }
+
+ public AnalogInSim getSimObject() {
+ return new AnalogInSim(m_channel);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
new file mode 100644
index 0000000..ae03463
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogOutSim;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Analog output class.
+ */
+public class AnalogOutput implements Sendable, AutoCloseable {
+ private int m_port;
+ private int m_channel;
+
+ /**
+ * Construct an analog output on a specified MXP channel.
+ *
+ * @param channel The channel number to represent.
+ */
+ public AnalogOutput(final int channel) {
+ SensorUtil.checkAnalogOutputChannel(channel);
+ m_channel = channel;
+
+ final int portHandle = HAL.getPort((byte) channel);
+ m_port = AnalogJNI.initializeAnalogOutputPort(portHandle);
+
+ HAL.report(tResourceType.kResourceType_AnalogOutput, channel + 1);
+ SendableRegistry.addLW(this, "AnalogOutput", channel);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ AnalogJNI.freeAnalogOutputPort(m_port);
+ m_port = 0;
+ m_channel = 0;
+ }
+
+ /**
+ * Get the channel of this AnalogOutput.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ public void setVoltage(double voltage) {
+ AnalogJNI.setAnalogOutput(m_port, voltage);
+ }
+
+ public double getVoltage() {
+ return AnalogJNI.getAnalogOutput(m_port);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Analog Output");
+ builder.addDoubleProperty("Value", this::getVoltage, this::setVoltage);
+ }
+
+ public AnalogOutSim getSimObject() {
+ return new AnalogOutSim(m_channel);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
new file mode 100644
index 0000000..3a6f338
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -0,0 +1,167 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.interfaces.Potentiometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that
+ * corresponds to a position. The position is in whichever units you choose, by way of the scaling
+ * and offset constants passed to the constructor.
+ */
+public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseable {
+ private AnalogInput m_analogInput;
+ private boolean m_initAnalogInput;
+ private double m_fullRange;
+ private double m_offset;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees. This will calculate the result from the fullRange times
+ * the fraction of the supply voltage, plus the offset.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
+ * @param offset The offset to add to the scaled value for controlling the zero value
+ */
+ public AnalogPotentiometer(final int channel, double fullRange, double offset) {
+ this(new AnalogInput(channel), fullRange, offset);
+ m_initAnalogInput = true;
+ SendableRegistry.addChild(this, m_analogInput);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees. This will calculate the result from the fullRange times
+ * the fraction of the supply voltage, plus the offset.
+ *
+ * @param input The {@link AnalogInput} this potentiometer is plugged into.
+ * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
+ * @param offset The offset to add to the scaled value for controlling the zero value
+ */
+ public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
+ SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel());
+ m_analogInput = input;
+ m_initAnalogInput = false;
+
+ m_fullRange = fullRange;
+ m_offset = offset;
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+ */
+ public AnalogPotentiometer(final int channel, double scale) {
+ this(channel, scale, 0);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees.
+ *
+ * @param input The {@link AnalogInput} this potentiometer is plugged into.
+ * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+ */
+ public AnalogPotentiometer(final AnalogInput input, double scale) {
+ this(input, scale, 0);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ */
+ public AnalogPotentiometer(final int channel) {
+ this(channel, 1, 0);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * @param input The {@link AnalogInput} this potentiometer is plugged into.
+ */
+ public AnalogPotentiometer(final AnalogInput input) {
+ this(input, 1, 0);
+ }
+
+ /**
+ * Get the current reading of the potentiometer.
+ *
+ * @return The current position of the potentiometer.
+ */
+ @Override
+ public double get() {
+ if (m_analogInput == null) {
+ return m_offset;
+ }
+ return (m_analogInput.getVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+ throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
+ }
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+ @Override
+ public double pidGet() {
+ return get();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ if (m_analogInput != null) {
+ m_analogInput.initSendable(builder);
+ }
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_initAnalogInput) {
+ m_analogInput.close();
+ m_analogInput = null;
+ m_initAnalogInput = false;
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
new file mode 100644
index 0000000..f7f98bd
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -0,0 +1,210 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.BoundaryException;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+
+/**
+ * Class for creating and configuring Analog Triggers.
+ */
+public class AnalogTrigger implements Sendable, AutoCloseable {
+ /**
+ * Exceptions dealing with improper operation of the Analog trigger.
+ */
+ public static class AnalogTriggerException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ public AnalogTriggerException(String message) {
+ super(message);
+ }
+
+ }
+
+ /**
+ * Where the analog trigger is attached.
+ */
+ protected int m_port;
+ protected AnalogInput m_analogInput;
+ protected DutyCycle m_dutyCycle;
+ protected boolean m_ownsAnalog;
+
+ /**
+ * Constructor for an analog trigger given a channel number.
+ *
+ * @param channel the port to use for the analog trigger
+ */
+ public AnalogTrigger(final int channel) {
+ this(new AnalogInput(channel));
+ m_ownsAnalog = true;
+ SendableRegistry.addChild(this, m_analogInput);
+ }
+
+ /**
+ * Construct an analog trigger given an analog channel. This should be used in the case of sharing
+ * an analog channel between the trigger and an analog input object.
+ *
+ * @param channel the AnalogInput to use for the analog trigger
+ */
+ public AnalogTrigger(AnalogInput channel) {
+ m_analogInput = channel;
+
+ m_port = AnalogJNI.initializeAnalogTrigger(channel.m_port);
+
+ int index = getIndex();
+
+ HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
+ SendableRegistry.addLW(this, "AnalogTrigger", index);
+ }
+
+ /**
+ * Construct an analog trigger given a duty cycle input.
+ *
+ * @param input the DutyCycle to use for the analog trigger
+ */
+ public AnalogTrigger(DutyCycle input) {
+ m_dutyCycle = input;
+
+ m_port = AnalogJNI.initializeAnalogTriggerDutyCycle(input.m_handle);
+
+ int index = getIndex();
+
+ HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
+ SendableRegistry.addLW(this, "AnalogTrigger", index);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ AnalogJNI.cleanAnalogTrigger(m_port);
+ m_port = 0;
+ if (m_ownsAnalog && m_analogInput != null) {
+ m_analogInput.close();
+ }
+ }
+
+ /**
+ * Set the upper and lower limits of the analog trigger. The limits are given in ADC codes. If
+ * oversampling is used, the units must be scaled appropriately.
+ *
+ * @param lower the lower raw limit
+ * @param upper the upper raw limit
+ */
+ public void setLimitsRaw(final int lower, final int upper) {
+ if (lower > upper) {
+ throw new BoundaryException("Lower bound is greater than upper");
+ }
+ AnalogJNI.setAnalogTriggerLimitsRaw(m_port, lower, upper);
+ }
+
+ /**
+ * Set the upper and lower limits of the analog trigger. The limits are given as floating point
+ * values between 0 and 1.
+ *
+ * @param lower the lower duty cycle limit
+ * @param upper the upper duty cycle limit
+ */
+ public void setLimitsDutyCycle(double lower, double upper) {
+ if (lower > upper) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ AnalogJNI.setAnalogTriggerLimitsDutyCycle(m_port, lower, upper);
+ }
+
+ /**
+ * Set the upper and lower limits of the analog trigger. The limits are given as floating point
+ * voltage values.
+ *
+ * @param lower the lower voltage limit
+ * @param upper the upper voltage limit
+ */
+ public void setLimitsVoltage(double lower, double upper) {
+ if (lower > upper) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ AnalogJNI.setAnalogTriggerLimitsVoltage(m_port, lower, upper);
+ }
+
+ /**
+ * Configure the analog trigger to use the averaged vs. raw values. If the value is true, then the
+ * averaged value is selected for the analog trigger, otherwise the immediate value is used.
+ *
+ * @param useAveragedValue true to use an averaged value, false otherwise
+ */
+ public void setAveraged(boolean useAveragedValue) {
+ AnalogJNI.setAnalogTriggerAveraged(m_port, useAveragedValue);
+ }
+
+ /**
+ * Configure the analog trigger to use a filtered value. The analog trigger will operate with a 3
+ * point average rejection filter. This is designed to help with 360 degree pot applications for
+ * the period where the pot crosses through zero.
+ *
+ * @param useFilteredValue true to use a filtered value, false otherwise
+ */
+ public void setFiltered(boolean useFilteredValue) {
+ AnalogJNI.setAnalogTriggerFiltered(m_port, useFilteredValue);
+ }
+
+ /**
+ * Return the index of the analog trigger. This is the FPGA index of this analog trigger
+ * instance.
+ *
+ * @return The index of the analog trigger.
+ */
+ public final int getIndex() {
+ return AnalogJNI.getAnalogTriggerFPGAIndex(m_port);
+ }
+
+ /**
+ * Return the InWindow output of the analog trigger. True if the analog input is between the upper
+ * and lower limits.
+ *
+ * @return The InWindow output of the analog trigger.
+ */
+ public boolean getInWindow() {
+ return AnalogJNI.getAnalogTriggerInWindow(m_port);
+ }
+
+ /**
+ * Return the TriggerState output of the analog trigger. True if above upper limit. False if below
+ * lower limit. If in Hysteresis, maintain previous state.
+ *
+ * @return The TriggerState output of the analog trigger.
+ */
+ public boolean getTriggerState() {
+ return AnalogJNI.getAnalogTriggerTriggerState(m_port);
+ }
+
+ /**
+ * Creates an AnalogTriggerOutput object. Gets an output object that can be used for routing.
+ * Caller is responsible for deleting the AnalogTriggerOutput object.
+ *
+ * @param type An enum of the type of output object to create.
+ * @return A pointer to a new AnalogTriggerOutput object.
+ */
+ public AnalogTriggerOutput createOutput(AnalogTriggerType type) {
+ return new AnalogTriggerOutput(this, type);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ if (m_ownsAnalog) {
+ m_analogInput.initSendable(builder);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
new file mode 100644
index 0000000..7635196
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 routing of an output to digital
+ * subsystems on the FPGA such as Counter, Encoder, and Interrupt.
+ *
+ * <p>The TriggerState output indicates the primary output value of the trigger. If the analog
+ * signal is less than the lower limit, the output is false. If the analog value is greater than the
+ * upper limit, then the output is true. If the analog value is in between, then the trigger output
+ * state maintains its most recent value.
+ *
+ * <p>The InWindow output indicates whether or not the analog signal is inside the range defined by
+ * the limits.
+ *
+ * <p>The RisingPulse and FallingPulse outputs detect an instantaneous transition from above the
+ * upper limit to below the lower limit, and vise versa. These pulses represent a rollover condition
+ * of a sensor and can be routed to an up / down counter or to interrupts. Because the outputs
+ * generate a pulse, they cannot be read directly. To help ensure that a rollover condition is not
+ * missed, there is an average rejection filter available that operates on the upper 8 bits of a 12
+ * bit number and selects the nearest outlier of 3 samples. This will reject a sample that is (due
+ * to averaging or sampling) errantly between the two limits. This filter will fail if more than one
+ * sample in a row is errantly in between the two limits. You may see this problem if attempting to
+ * use this feature with a mechanical rollover sensor, such as a 360 degree no-stop potentiometer
+ * without signal conditioning, because the rollover transition is not sharp / clean enough. Using
+ * the averaging engine may help with this, but rotational speeds of the sensor will then be
+ * limited.
+ */
+public class AnalogTriggerOutput extends DigitalSource implements Sendable {
+ /**
+ * Exceptions dealing with improper operation of the Analog trigger output.
+ */
+ public static class AnalogTriggerOutputException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ public AnalogTriggerOutputException(String message) {
+ super(message);
+ }
+ }
+
+ private final AnalogTrigger m_trigger;
+ private final AnalogTriggerType m_outputType;
+
+ /**
+ * Create an object that represents one of the four outputs from an analog trigger.
+ *
+ * <p>Because this class derives from DigitalSource, it can be passed into routing functions for
+ * Counter, Encoder, etc.
+ *
+ * @param trigger The trigger for which this is an output.
+ * @param outputType An enum that specifies the output on the trigger to represent.
+ */
+ public AnalogTriggerOutput(AnalogTrigger trigger, final AnalogTriggerType outputType) {
+ requireNonNullParam(trigger, "trigger", "AnalogTriggerOutput");
+ requireNonNullParam(outputType, "outputType", "AnalogTriggerOutput");
+
+ m_trigger = trigger;
+ m_outputType = outputType;
+ HAL.report(tResourceType.kResourceType_AnalogTriggerOutput,
+ trigger.getIndex() + 1, outputType.value + 1);
+ }
+
+ /**
+ * Get the state of the analog trigger output.
+ *
+ * @return The state of the analog trigger output.
+ */
+ public boolean get() {
+ return AnalogJNI.getAnalogTriggerOutput(m_trigger.m_port, m_outputType.value);
+ }
+
+ @Override
+ public int getPortHandleForRouting() {
+ return m_trigger.m_port;
+ }
+
+ @Override
+ public int getAnalogTriggerTypeForRouting() {
+ return m_outputType.value;
+ }
+
+ @Override
+ public int getChannel() {
+ return m_trigger.getIndex();
+ }
+
+ @Override
+ public boolean isAnalogTrigger() {
+ return true;
+ }
+
+ /**
+ * Defines the state in which the AnalogTrigger triggers.
+ */
+ public enum AnalogTriggerType {
+ kInWindow(AnalogJNI.AnalogTriggerType.kInWindow), kState(AnalogJNI.AnalogTriggerType.kState),
+ kRisingPulse(AnalogJNI.AnalogTriggerType.kRisingPulse),
+ kFallingPulse(AnalogJNI.AnalogTriggerType.kFallingPulse);
+
+ @SuppressWarnings("MemberName")
+ private final int value;
+
+ AnalogTriggerType(int value) {
+ this.value = value;
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
new file mode 100644
index 0000000..f6811c0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AccelerometerJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AccelerometerSim;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Built-in accelerometer.
+ *
+ * <p>This class allows access to the roboRIO's internal accelerometer.
+ */
+public class BuiltInAccelerometer implements Accelerometer, Sendable, AutoCloseable {
+ /**
+ * Constructor.
+ *
+ * @param range The range the accelerometer will measure
+ */
+ public BuiltInAccelerometer(Range range) {
+ setRange(range);
+ HAL.report(tResourceType.kResourceType_Accelerometer, 0, 0, "Built-in accelerometer");
+ SendableRegistry.addLW(this, "BuiltInAccel", 0);
+ }
+
+ /**
+ * Constructor. The accelerometer will measure +/-8 g-forces
+ */
+ public BuiltInAccelerometer() {
+ this(Range.k8G);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ @Override
+ public void setRange(Range range) {
+ AccelerometerJNI.setAccelerometerActive(false);
+
+ switch (range) {
+ case k2G:
+ AccelerometerJNI.setAccelerometerRange(0);
+ break;
+ case k4G:
+ AccelerometerJNI.setAccelerometerRange(1);
+ break;
+ case k8G:
+ AccelerometerJNI.setAccelerometerRange(2);
+ break;
+ case k16G:
+ default:
+ throw new IllegalArgumentException(range + " range not supported (use k2G, k4G, or k8G)");
+
+ }
+
+ AccelerometerJNI.setAccelerometerActive(true);
+ }
+
+ /**
+ * The acceleration in the X axis.
+ *
+ * @return The acceleration of the roboRIO along the X axis in g-forces
+ */
+ @Override
+ public double getX() {
+ return AccelerometerJNI.getAccelerometerX();
+ }
+
+ /**
+ * The acceleration in the Y axis.
+ *
+ * @return The acceleration of the roboRIO along the Y axis in g-forces
+ */
+ @Override
+ public double getY() {
+ return AccelerometerJNI.getAccelerometerY();
+ }
+
+ /**
+ * The acceleration in the Z axis.
+ *
+ * @return The acceleration of the roboRIO along the Z axis in g-forces
+ */
+ @Override
+ public double getZ() {
+ return AccelerometerJNI.getAccelerometerZ();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ builder.addDoubleProperty("X", this::getX, null);
+ builder.addDoubleProperty("Y", this::getY, null);
+ builder.addDoubleProperty("Z", this::getZ, null);
+ }
+
+ public AccelerometerSim getSimObject() {
+ return new AccelerometerSim();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
new file mode 100644
index 0000000..7b188e5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.Closeable;
+
+import edu.wpi.first.hal.CANAPIJNI;
+import edu.wpi.first.hal.CANData;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * High level class for interfacing with CAN devices conforming to
+ * the standard CAN spec.
+ *
+ * <p>No packets that can be sent gets blocked by the RoboRIO, so all methods
+ * work identically in all robot modes.
+ *
+ * <p>All methods are thread safe, however the CANData object passed into the
+ * read methods and the byte[] passed into the write methods need to not
+ * be modified for the duration of their respective calls.
+ */
+public class CAN implements Closeable {
+ public static final int kTeamManufacturer = 8;
+ public static final int kTeamDeviceType = 10;
+
+ private final int m_handle;
+
+ /**
+ * Create a new CAN communication interface with the specific device ID.
+ * This uses the team manufacturer and device types.
+ * The device ID is 6 bits (0-63).
+ *
+ * @param deviceId The device id
+ */
+ public CAN(int deviceId) {
+ m_handle = CANAPIJNI.initializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType);
+ HAL.report(tResourceType.kResourceType_CAN, deviceId + 1);
+ }
+
+ /**
+ * Create a new CAN communication interface with a specific device ID,
+ * manufacturer and device type. The device ID is 6 bits, the
+ * manufacturer is 8 bits, and the device type is 5 bits.
+ *
+ * @param deviceId The device ID
+ * @param deviceManufacturer The device manufacturer
+ * @param deviceType The device type
+ */
+ public CAN(int deviceId, int deviceManufacturer, int deviceType) {
+ m_handle = CANAPIJNI.initializeCAN(deviceManufacturer, deviceId, deviceType);
+ HAL.report(tResourceType.kResourceType_CAN, deviceId + 1);
+ }
+
+ /**
+ * Closes the CAN communication.
+ */
+ @Override
+ public void close() {
+ if (m_handle != 0) {
+ CANAPIJNI.cleanCAN(m_handle);
+ }
+ }
+
+ /**
+ * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+ *
+ * @param data The data to write (8 bytes max)
+ * @param apiId The API ID to write.
+ */
+ public void writePacket(byte[] data, int apiId) {
+ CANAPIJNI.writeCANPacket(m_handle, data, apiId);
+ }
+
+ /**
+ * Write a repeating packet to the CAN device with a specific ID. This ID is 10 bits.
+ * The RoboRIO will automatically repeat the packet at the specified interval
+ *
+ * @param data The data to write (8 bytes max)
+ * @param apiId The API ID to write.
+ * @param repeatMs The period to repeat the packet at.
+ */
+ public void writePacketRepeating(byte[] data, int apiId, int repeatMs) {
+ CANAPIJNI.writeCANPacketRepeating(m_handle, data, apiId, repeatMs);
+ }
+
+ /**
+ * Write an RTR frame to the CAN device with a specific ID. This ID is 10 bits.
+ * The length by spec must match what is returned by the responding device
+ *
+ * @param length The length to request (0 to 8)
+ * @param apiId The API ID to write.
+ */
+ public void writeRTRFrame(int length, int apiId) {
+ CANAPIJNI.writeCANRTRFrame(m_handle, length, apiId);
+ }
+
+ /**
+ * Stop a repeating packet with a specific ID. This ID is 10 bits.
+ *
+ * @param apiId The API ID to stop repeating
+ */
+ public void stopPacketRepeating(int apiId) {
+ CANAPIJNI.stopCANPacketRepeating(m_handle, apiId);
+ }
+
+ /**
+ * Read a new CAN packet. This will only return properly once per packet
+ * received. Multiple calls without receiving another packet will return false.
+ *
+ * @param apiId The API ID to read.
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPacketNew(int apiId, CANData data) {
+ return CANAPIJNI.readCANPacketNew(m_handle, apiId, data);
+ }
+
+ /**
+ * Read a CAN packet. The will continuously return the last packet received,
+ * without accounting for packet age.
+ *
+ * @param apiId The API ID to read.
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPacketLatest(int apiId, CANData data) {
+ return CANAPIJNI.readCANPacketLatest(m_handle, apiId, data);
+ }
+
+ /**
+ * Read a CAN packet. The will return the last packet received until the
+ * packet is older then the requested timeout. Then it will return false.
+ *
+ * @param apiId The API ID to read.
+ * @param timeoutMs The timeout time for the packet
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPacketTimeout(int apiId, int timeoutMs, CANData data) {
+ return CANAPIJNI.readCANPacketTimeout(m_handle, apiId, timeoutMs, data);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
new file mode 100644
index 0000000..178d5ee
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
@@ -0,0 +1,784 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Hashtable;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+
+import edu.wpi.cscore.AxisCamera;
+import edu.wpi.cscore.CameraServerJNI;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.MjpegServer;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoEvent;
+import edu.wpi.cscore.VideoException;
+import edu.wpi.cscore.VideoListener;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.cscore.VideoMode.PixelFormat;
+import edu.wpi.cscore.VideoProperty;
+import edu.wpi.cscore.VideoSink;
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * Singleton class for creating and keeping camera servers.
+ * Also publishes camera information to NetworkTables.
+ *
+ * @deprecated Replaced with edu.wpi.first.cameraserver.CameraServer
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+@Deprecated
+public final class CameraServer {
+ public static final int kBasePort = 1181;
+
+ @Deprecated
+ public static final int kSize640x480 = 0;
+ @Deprecated
+ public static final int kSize320x240 = 1;
+ @Deprecated
+ public static final int kSize160x120 = 2;
+
+ private static final String kPublishName = "/CameraPublisher";
+ private static CameraServer server;
+
+ /**
+ * Get the CameraServer instance.
+ */
+ public static synchronized CameraServer getInstance() {
+ if (server == null) {
+ server = new CameraServer();
+ }
+ return server;
+ }
+
+ private final AtomicInteger m_defaultUsbDevice;
+ private String m_primarySourceName;
+ private final Map<String, VideoSource> m_sources;
+ private final Map<String, VideoSink> m_sinks;
+ private final Map<Integer, NetworkTable> m_tables; // indexed by source handle
+ private final NetworkTable m_publishTable;
+ private final VideoListener m_videoListener; //NOPMD
+ private final int m_tableListener; //NOPMD
+ private int m_nextPort;
+ private String[] m_addresses;
+
+ @SuppressWarnings("JavadocMethod")
+ private static String makeSourceValue(int source) {
+ switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
+ case kUsb:
+ return "usb:" + CameraServerJNI.getUsbCameraPath(source);
+ case kHttp: {
+ String[] urls = CameraServerJNI.getHttpCameraUrls(source);
+ if (urls.length > 0) {
+ return "ip:" + urls[0];
+ } else {
+ return "ip:";
+ }
+ }
+ case kCv:
+ // FIXME: Should be "cv:", but LabVIEW dashboard requires "usb:".
+ // https://github.com/wpilibsuite/allwpilib/issues/407
+ return "usb:";
+ default:
+ return "unknown:";
+ }
+ }
+
+ @SuppressWarnings("JavadocMethod")
+ private static String makeStreamValue(String address, int port) {
+ return "mjpg:http://" + address + ":" + port + "/?action=stream";
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+ private synchronized String[] getSinkStreamValues(int sink) {
+ // Ignore all but MjpegServer
+ if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
+ return new String[0];
+ }
+
+ // Get port
+ int port = CameraServerJNI.getMjpegServerPort(sink);
+
+ // Generate values
+ ArrayList<String> values = new ArrayList<>(m_addresses.length + 1);
+ String listenAddress = CameraServerJNI.getMjpegServerListenAddress(sink);
+ if (!listenAddress.isEmpty()) {
+ // If a listen address is specified, only use that
+ values.add(makeStreamValue(listenAddress, port));
+ } else {
+ // Otherwise generate for hostname and all interface addresses
+ values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
+ for (String addr : m_addresses) {
+ if ("127.0.0.1".equals(addr)) {
+ continue; // ignore localhost
+ }
+ values.add(makeStreamValue(addr, port));
+ }
+ }
+
+ return values.toArray(new String[0]);
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+ private synchronized String[] getSourceStreamValues(int source) {
+ // Ignore all but HttpCamera
+ if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
+ != VideoSource.Kind.kHttp) {
+ return new String[0];
+ }
+
+ // Generate values
+ String[] values = CameraServerJNI.getHttpCameraUrls(source);
+ for (int j = 0; j < values.length; j++) {
+ values[j] = "mjpg:" + values[j];
+ }
+
+ if (CameraServerSharedStore.getCameraServerShared().isRoboRIO()) {
+ // Look to see if we have a passthrough server for this source
+ // Only do this on the roboRIO
+ for (VideoSink i : m_sinks.values()) {
+ int sink = i.getHandle();
+ int sinkSource = CameraServerJNI.getSinkSource(sink);
+ if (source == sinkSource
+ && VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink))
+ == VideoSink.Kind.kMjpeg) {
+ // Add USB-only passthrough
+ String[] finalValues = Arrays.copyOf(values, values.length + 1);
+ int port = CameraServerJNI.getMjpegServerPort(sink);
+ finalValues[values.length] = makeStreamValue("172.22.11.2", port);
+ return finalValues;
+ }
+ }
+ }
+
+ return values;
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+ private synchronized void updateStreamValues() {
+ // Over all the sinks...
+ for (VideoSink i : m_sinks.values()) {
+ int sink = i.getHandle();
+
+ // Get the source's subtable (if none exists, we're done)
+ int source = CameraServerJNI.getSinkSource(sink);
+ if (source == 0) {
+ continue;
+ }
+ NetworkTable table = m_tables.get(source);
+ if (table != null) {
+ // Don't set stream values if this is a HttpCamera passthrough
+ if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
+ == VideoSource.Kind.kHttp) {
+ continue;
+ }
+
+ // Set table value
+ String[] values = getSinkStreamValues(sink);
+ if (values.length > 0) {
+ table.getEntry("streams").setStringArray(values);
+ }
+ }
+ }
+
+ // Over all the sources...
+ for (VideoSource i : m_sources.values()) {
+ int source = i.getHandle();
+
+ // Get the source's subtable (if none exists, we're done)
+ NetworkTable table = m_tables.get(source);
+ if (table != null) {
+ // Set table value
+ String[] values = getSourceStreamValues(source);
+ if (values.length > 0) {
+ table.getEntry("streams").setStringArray(values);
+ }
+ }
+ }
+ }
+
+ @SuppressWarnings("JavadocMethod")
+ private static String pixelFormatToString(PixelFormat pixelFormat) {
+ switch (pixelFormat) {
+ case kMJPEG:
+ return "MJPEG";
+ case kYUYV:
+ return "YUYV";
+ case kRGB565:
+ return "RGB565";
+ case kBGR:
+ return "BGR";
+ case kGray:
+ return "Gray";
+ default:
+ return "Unknown";
+ }
+ }
+
+ /// Provide string description of video mode.
+ /// The returned string is "{width}x{height} {format} {fps} fps".
+ @SuppressWarnings("JavadocMethod")
+ private static String videoModeToString(VideoMode mode) {
+ return mode.width + "x" + mode.height + " " + pixelFormatToString(mode.pixelFormat)
+ + " " + mode.fps + " fps";
+ }
+
+ @SuppressWarnings("JavadocMethod")
+ private static String[] getSourceModeValues(int sourceHandle) {
+ VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
+ String[] modeStrings = new String[modes.length];
+ for (int i = 0; i < modes.length; i++) {
+ modeStrings[i] = videoModeToString(modes[i]);
+ }
+ return modeStrings;
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.CyclomaticComplexity"})
+ private static void putSourcePropertyValue(NetworkTable table, VideoEvent event, boolean isNew) {
+ String name;
+ String infoName;
+ if (event.name.startsWith("raw_")) {
+ name = "RawProperty/" + event.name;
+ infoName = "RawPropertyInfo/" + event.name;
+ } else {
+ name = "Property/" + event.name;
+ infoName = "PropertyInfo/" + event.name;
+ }
+
+ NetworkTableEntry entry = table.getEntry(name);
+ try {
+ switch (event.propertyKind) {
+ case kBoolean:
+ if (isNew) {
+ entry.setDefaultBoolean(event.value != 0);
+ } else {
+ entry.setBoolean(event.value != 0);
+ }
+ break;
+ case kInteger:
+ case kEnum:
+ if (isNew) {
+ entry.setDefaultDouble(event.value);
+ table.getEntry(infoName + "/min").setDouble(
+ CameraServerJNI.getPropertyMin(event.propertyHandle));
+ table.getEntry(infoName + "/max").setDouble(
+ CameraServerJNI.getPropertyMax(event.propertyHandle));
+ table.getEntry(infoName + "/step").setDouble(
+ CameraServerJNI.getPropertyStep(event.propertyHandle));
+ table.getEntry(infoName + "/default").setDouble(
+ CameraServerJNI.getPropertyDefault(event.propertyHandle));
+ } else {
+ entry.setDouble(event.value);
+ }
+ break;
+ case kString:
+ if (isNew) {
+ entry.setDefaultString(event.valueStr);
+ } else {
+ entry.setString(event.valueStr);
+ }
+ break;
+ default:
+ break;
+ }
+ } catch (VideoException ignored) {
+ // ignore
+ }
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.UnusedLocalVariable", "PMD.ExcessiveMethodLength",
+ "PMD.NPathComplexity"})
+ private CameraServer() {
+ m_defaultUsbDevice = new AtomicInteger();
+ m_sources = new Hashtable<>();
+ m_sinks = new Hashtable<>();
+ m_tables = new Hashtable<>();
+ m_publishTable = NetworkTableInstance.getDefault().getTable(kPublishName);
+ m_nextPort = kBasePort;
+ m_addresses = new String[0];
+
+ // We publish sources to NetworkTables using the following structure:
+ // "/CameraPublisher/{Source.Name}/" - root
+ // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
+ // - "streams" (string array): URLs that can be used to stream data
+ // - "description" (string): Description of the source
+ // - "connected" (boolean): Whether source is connected
+ // - "mode" (string): Current video mode
+ // - "modes" (string array): Available video modes
+ // - "Property/{Property}" - Property values
+ // - "PropertyInfo/{Property}" - Property supporting information
+
+ // Listener for video events
+ m_videoListener = new VideoListener(event -> {
+ switch (event.kind) {
+ case kSourceCreated: {
+ // Create subtable for the camera
+ NetworkTable table = m_publishTable.getSubTable(event.name);
+ m_tables.put(event.sourceHandle, table);
+ table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
+ table.getEntry("description").setString(
+ CameraServerJNI.getSourceDescription(event.sourceHandle));
+ table.getEntry("connected").setBoolean(
+ CameraServerJNI.isSourceConnected(event.sourceHandle));
+ table.getEntry("streams").setStringArray(getSourceStreamValues(event.sourceHandle));
+ try {
+ VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
+ table.getEntry("mode").setDefaultString(videoModeToString(mode));
+ table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
+ } catch (VideoException ignored) {
+ // Do nothing. Let the other event handlers update this if there is an error.
+ }
+ break;
+ }
+ case kSourceDestroyed: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("source").setString("");
+ table.getEntry("streams").setStringArray(new String[0]);
+ table.getEntry("modes").setStringArray(new String[0]);
+ }
+ break;
+ }
+ case kSourceConnected: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ // update the description too (as it may have changed)
+ table.getEntry("description").setString(
+ CameraServerJNI.getSourceDescription(event.sourceHandle));
+ table.getEntry("connected").setBoolean(true);
+ }
+ break;
+ }
+ case kSourceDisconnected: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("connected").setBoolean(false);
+ }
+ break;
+ }
+ case kSourceVideoModesUpdated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
+ }
+ break;
+ }
+ case kSourceVideoModeChanged: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("mode").setString(videoModeToString(event.mode));
+ }
+ break;
+ }
+ case kSourcePropertyCreated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ putSourcePropertyValue(table, event, true);
+ }
+ break;
+ }
+ case kSourcePropertyValueUpdated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ putSourcePropertyValue(table, event, false);
+ }
+ break;
+ }
+ case kSourcePropertyChoicesUpdated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ try {
+ String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
+ table.getEntry("PropertyInfo/" + event.name + "/choices").setStringArray(choices);
+ } catch (VideoException ignored) {
+ // ignore
+ }
+ }
+ break;
+ }
+ case kSinkSourceChanged:
+ case kSinkCreated:
+ case kSinkDestroyed:
+ case kNetworkInterfacesChanged: {
+ m_addresses = CameraServerJNI.getNetworkInterfaces();
+ updateStreamValues();
+ break;
+ }
+ default:
+ break;
+ }
+ }, 0x4fff, true);
+
+ // Listener for NetworkTable events
+ // We don't currently support changing settings via NT due to
+ // synchronization issues, so just update to current setting if someone
+ // else tries to change it.
+ m_tableListener = NetworkTableInstance.getDefault().addEntryListener(kPublishName + "/",
+ event -> {
+ String relativeKey = event.name.substring(kPublishName.length() + 1);
+
+ // get source (sourceName/...)
+ int subKeyIndex = relativeKey.indexOf('/');
+ if (subKeyIndex == -1) {
+ return;
+ }
+ String sourceName = relativeKey.substring(0, subKeyIndex);
+ VideoSource source = m_sources.get(sourceName);
+ if (source == null) {
+ return;
+ }
+
+ // get subkey
+ relativeKey = relativeKey.substring(subKeyIndex + 1);
+
+ // handle standard names
+ String propName;
+ if ("mode".equals(relativeKey)) {
+ // reset to current mode
+ event.getEntry().setString(videoModeToString(source.getVideoMode()));
+ return;
+ } else if (relativeKey.startsWith("Property/")) {
+ propName = relativeKey.substring(9);
+ } else if (relativeKey.startsWith("RawProperty/")) {
+ propName = relativeKey.substring(12);
+ } else {
+ return; // ignore
+ }
+
+ // everything else is a property
+ VideoProperty property = source.getProperty(propName);
+ switch (property.getKind()) {
+ case kNone:
+ return;
+ case kBoolean:
+ // reset to current setting
+ event.getEntry().setBoolean(property.get() != 0);
+ return;
+ case kInteger:
+ case kEnum:
+ // reset to current setting
+ event.getEntry().setDouble(property.get());
+ return;
+ case kString:
+ // reset to current setting
+ event.getEntry().setString(property.getString());
+ return;
+ default:
+ return;
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * <p>You should call this method to see a camera feed on the dashboard.
+ * If you also want to perform vision processing on the roboRIO, use
+ * getVideo() to get access to the camera images.
+ *
+ * <p>The first time this overload is called, it calls
+ * {@link #startAutomaticCapture(int)} with device 0, creating a camera
+ * named "USB Camera 0". Subsequent calls increment the device number
+ * (e.g. 1, 2, etc).
+ */
+ public UsbCamera startAutomaticCapture() {
+ UsbCamera camera = startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * <p>This overload calls {@link #startAutomaticCapture(String, int)} with
+ * a name of "USB Camera {dev}".
+ *
+ * @param dev The device number of the camera interface
+ */
+ public UsbCamera startAutomaticCapture(int dev) {
+ UsbCamera camera = new UsbCamera("USB Camera " + dev, dev);
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * @param name The name to give the camera
+ * @param dev The device number of the camera interface
+ */
+ public UsbCamera startAutomaticCapture(String name, int dev) {
+ UsbCamera camera = new UsbCamera(name, dev);
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * @param name The name to give the camera
+ * @param path The device path (e.g. "/dev/video0") of the camera
+ */
+ public UsbCamera startAutomaticCapture(String name, String path) {
+ UsbCamera camera = new UsbCamera(name, path);
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard from
+ * an existing camera.
+ *
+ * @param camera Camera
+ */
+ public void startAutomaticCapture(VideoSource camera) {
+ addCamera(camera);
+ VideoSink server = addServer("serve_" + camera.getName());
+ server.setSource(camera);
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * <p>This overload calls {@link #addAxisCamera(String, String)} with
+ * name "Axis Camera".
+ *
+ * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+ */
+ public AxisCamera addAxisCamera(String host) {
+ return addAxisCamera("Axis Camera", host);
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * <p>This overload calls {@link #addAxisCamera(String, String[])} with
+ * name "Axis Camera".
+ *
+ * @param hosts Array of Camera host IPs/DNS names
+ */
+ public AxisCamera addAxisCamera(String[] hosts) {
+ return addAxisCamera("Axis Camera", hosts);
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * @param name The name to give the camera
+ * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+ */
+ public AxisCamera addAxisCamera(String name, String host) {
+ AxisCamera camera = new AxisCamera(name, host);
+ // Create a passthrough MJPEG server for USB access
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * @param name The name to give the camera
+ * @param hosts Array of Camera host IPs/DNS names
+ */
+ public AxisCamera addAxisCamera(String name, String[] hosts) {
+ AxisCamera camera = new AxisCamera(name, hosts);
+ // Create a passthrough MJPEG server for USB access
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Get OpenCV access to the primary camera feed. This allows you to
+ * get images from the camera for image processing on the roboRIO.
+ *
+ * <p>This is only valid to call after a camera feed has been added
+ * with startAutomaticCapture() or addServer().
+ */
+ public CvSink getVideo() {
+ VideoSource source;
+ synchronized (this) {
+ if (m_primarySourceName == null) {
+ throw new VideoException("no camera available");
+ }
+ source = m_sources.get(m_primarySourceName);
+ }
+ if (source == null) {
+ throw new VideoException("no camera available");
+ }
+ return getVideo(source);
+ }
+
+ /**
+ * Get OpenCV access to the specified camera. This allows you to get
+ * images from the camera for image processing on the roboRIO.
+ *
+ * @param camera Camera (e.g. as returned by startAutomaticCapture).
+ */
+ public CvSink getVideo(VideoSource camera) {
+ String name = "opencv_" + camera.getName();
+
+ synchronized (this) {
+ VideoSink sink = m_sinks.get(name);
+ if (sink != null) {
+ VideoSink.Kind kind = sink.getKind();
+ if (kind != VideoSink.Kind.kCv) {
+ throw new VideoException("expected OpenCV sink, but got " + kind);
+ }
+ return (CvSink) sink;
+ }
+ }
+
+ CvSink newsink = new CvSink(name);
+ newsink.setSource(camera);
+ addServer(newsink);
+ return newsink;
+ }
+
+ /**
+ * Get OpenCV access to the specified camera. This allows you to get
+ * images from the camera for image processing on the roboRIO.
+ *
+ * @param name Camera name
+ */
+ public CvSink getVideo(String name) {
+ VideoSource source;
+ synchronized (this) {
+ source = m_sources.get(name);
+ if (source == null) {
+ throw new VideoException("could not find camera " + name);
+ }
+ }
+ return getVideo(source);
+ }
+
+ /**
+ * Create a MJPEG stream with OpenCV input. This can be called to pass custom
+ * annotated images to the dashboard.
+ *
+ * @param name Name to give the stream
+ * @param width Width of the image being sent
+ * @param height Height of the image being sent
+ */
+ public CvSource putVideo(String name, int width, int height) {
+ CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
+ startAutomaticCapture(source);
+ return source;
+ }
+
+ /**
+ * Adds a MJPEG server at the next available port.
+ *
+ * @param name Server name
+ */
+ public MjpegServer addServer(String name) {
+ int port;
+ synchronized (this) {
+ port = m_nextPort;
+ m_nextPort++;
+ }
+ return addServer(name, port);
+ }
+
+ /**
+ * Adds a MJPEG server.
+ *
+ * @param name Server name
+ */
+ public MjpegServer addServer(String name, int port) {
+ MjpegServer server = new MjpegServer(name, port);
+ addServer(server);
+ return server;
+ }
+
+ /**
+ * Adds an already created server.
+ *
+ * @param server Server
+ */
+ public void addServer(VideoSink server) {
+ synchronized (this) {
+ m_sinks.put(server.getName(), server);
+ }
+ }
+
+ /**
+ * Removes a server by name.
+ *
+ * @param name Server name
+ */
+ public void removeServer(String name) {
+ synchronized (this) {
+ m_sinks.remove(name);
+ }
+ }
+
+ /**
+ * Get server for the primary camera feed.
+ *
+ * <p>This is only valid to call after a camera feed has been added
+ * with startAutomaticCapture() or addServer().
+ */
+ public VideoSink getServer() {
+ synchronized (this) {
+ if (m_primarySourceName == null) {
+ throw new VideoException("no camera available");
+ }
+ return getServer("serve_" + m_primarySourceName);
+ }
+ }
+
+ /**
+ * Gets a server by name.
+ *
+ * @param name Server name
+ */
+ public VideoSink getServer(String name) {
+ synchronized (this) {
+ return m_sinks.get(name);
+ }
+ }
+
+ /**
+ * Adds an already created camera.
+ *
+ * @param camera Camera
+ */
+ public void addCamera(VideoSource camera) {
+ String name = camera.getName();
+ synchronized (this) {
+ if (m_primarySourceName == null) {
+ m_primarySourceName = name;
+ }
+ m_sources.put(name, camera);
+ }
+ }
+
+ /**
+ * Removes a camera by name.
+ *
+ * @param name Camera name
+ */
+ public void removeCamera(String name) {
+ synchronized (this) {
+ m_sources.remove(name);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
new file mode 100644
index 0000000..9d8a8dd
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.CompressorJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for operating a compressor connected to a PCM (Pneumatic Control Module). The PCM will
+ * automatically run in closed loop mode by default whenever a {@link Solenoid} object is created.
+ * For most cases, a Compressor object does not need to be instantiated or used in a robot program.
+ * This class is only required in cases where the robot program needs a more detailed status of the
+ * compressor or to enable/disable closed loop control.
+ *
+ * <p>Note: you cannot operate the compressor directly from this class as doing so would circumvent
+ * the safety provided by using the pressure switch and closed loop control. You can only turn off
+ * closed loop control, thereby stopping the compressor from operating.
+ */
+public class Compressor implements Sendable, AutoCloseable {
+ private int m_compressorHandle;
+ private byte m_module;
+
+ /**
+ * Makes a new instance of the compressor using the provided CAN device ID. Use this constructor
+ * when you have more than one PCM.
+ *
+ * @param module The PCM CAN device ID (0 - 62 inclusive)
+ */
+ public Compressor(int module) {
+ m_module = (byte) module;
+
+ m_compressorHandle = CompressorJNI.initializeCompressor((byte) module);
+
+ HAL.report(tResourceType.kResourceType_Compressor, module + 1);
+ SendableRegistry.addLW(this, "Compressor", module);
+ }
+
+ /**
+ * Makes a new instance of the compressor using the default PCM ID of 0.
+ *
+ * <p>Additional modules can be supported by making a new instance and {@link #Compressor(int)
+ * specifying the CAN ID.}
+ */
+ public Compressor() {
+ this(SensorUtil.getDefaultSolenoidModule());
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Start the compressor running in closed loop control mode.
+ *
+ * <p>Use the method in cases where you would like to manually stop and start the compressor for
+ * applications such as conserving battery or making sure that the compressor motor doesn't start
+ * during critical operations.
+ */
+ public void start() {
+ setClosedLoopControl(true);
+ }
+
+ /**
+ * Stop the compressor from running in closed loop control mode.
+ *
+ * <p>Use the method in cases where you would like to manually stop and start the compressor for
+ * applications such as conserving battery or making sure that the compressor motor doesn't start
+ * during critical operations.
+ */
+ public void stop() {
+ setClosedLoopControl(false);
+ }
+
+ /**
+ * Get the status of the compressor.
+ *
+ * @return true if the compressor is on
+ */
+ public boolean enabled() {
+ return CompressorJNI.getCompressor(m_compressorHandle);
+ }
+
+ /**
+ * Get the pressure switch value.
+ *
+ * @return true if the pressure is low
+ */
+ public boolean getPressureSwitchValue() {
+ return CompressorJNI.getCompressorPressureSwitch(m_compressorHandle);
+ }
+
+ /**
+ * Get the current being used by the compressor.
+ *
+ * @return current consumed by the compressor in amps
+ */
+ public double getCompressorCurrent() {
+ return CompressorJNI.getCompressorCurrent(m_compressorHandle);
+ }
+
+ /**
+ * Set the PCM in closed loop control mode.
+ *
+ * @param on if true sets the compressor to be in closed loop control mode (default)
+ */
+ public void setClosedLoopControl(boolean on) {
+ CompressorJNI.setCompressorClosedLoopControl(m_compressorHandle, on);
+ }
+
+ /**
+ * Gets the current operating mode of the PCM.
+ *
+ * @return true if compressor is operating on closed-loop mode
+ */
+ public boolean getClosedLoopControl() {
+ return CompressorJNI.getCompressorClosedLoopControl(m_compressorHandle);
+ }
+
+ /**
+ * If PCM is in fault state : Compressor Drive is disabled due to compressor current being too
+ * high.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getCompressorCurrentTooHighFault() {
+ return CompressorJNI.getCompressorCurrentTooHighFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM sticky fault is set : Compressor is disabled due to compressor current being too
+ * high.
+ *
+ * @return true if PCM sticky fault is set.
+ */
+ public boolean getCompressorCurrentTooHighStickyFault() {
+ return CompressorJNI.getCompressorCurrentTooHighStickyFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM sticky fault is set : Compressor output appears to be shorted.
+ *
+ * @return true if PCM sticky fault is set.
+ */
+ public boolean getCompressorShortedStickyFault() {
+ return CompressorJNI.getCompressorShortedStickyFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM is in fault state : Compressor output appears to be shorted.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getCompressorShortedFault() {
+ return CompressorJNI.getCompressorShortedFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM sticky fault is set : Compressor does not appear to be wired, i.e. compressor is not
+ * drawing enough current.
+ *
+ * @return true if PCM sticky fault is set.
+ */
+ public boolean getCompressorNotConnectedStickyFault() {
+ return CompressorJNI.getCompressorNotConnectedStickyFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM is in fault state : Compressor does not appear to be wired, i.e. compressor is not
+ * drawing enough current.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getCompressorNotConnectedFault() {
+ return CompressorJNI.getCompressorNotConnectedFault(m_compressorHandle);
+ }
+
+ /**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * <p>If a sticky fault is set, then it will be persistently cleared. The compressor might
+ * momentarily disable while the flags are being cleared. Doo not call this method too
+ * frequently, otherwise normal compressor functionality may be prevented.
+ *
+ * <p>If no sticky faults are set then this call will have no effect.
+ */
+ public void clearAllPCMStickyFaults() {
+ CompressorJNI.clearAllPCMStickyFaults(m_module);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Compressor");
+ builder.addBooleanProperty("Enabled", this::enabled, value -> {
+ if (value) {
+ start();
+ } else {
+ stop();
+ }
+ });
+ builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
new file mode 100644
index 0000000..31d50d8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * An interface for controllers. Controllers run control loops, the most command are PID controllers
+ * and there variants, but this includes anything that is controlling an actuator in a separate
+ * thread.
+ *
+ * @deprecated None of the 2020 FRC controllers use this.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public interface Controller {
+ /**
+ * Allows the control loop to run.
+ */
+ void enable();
+
+ /**
+ * Stops the control loop from running until explicitly re-enabled by calling {@link #enable()}.
+ */
+ void disable();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
new file mode 100644
index 0000000..0a26810
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -0,0 +1,566 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.CounterJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Class for counting the number of ticks on a digital input channel.
+ *
+ * <p>This is a general purpose class for counting repetitive events. It can return the number of
+ * counts, the period of the most recent cycle, and detect when the signal being counted has
+ * stopped by supplying a maximum cycle time.
+ *
+ * <p>All counters will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable {
+ /**
+ * Mode determines how and what the counter counts.
+ */
+ public enum Mode {
+ /**
+ * mode: two pulse.
+ */
+ kTwoPulse(0),
+ /**
+ * mode: semi period.
+ */
+ kSemiperiod(1),
+ /**
+ * mode: pulse length.
+ */
+ kPulseLength(2),
+ /**
+ * mode: external direction.
+ */
+ kExternalDirection(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Mode(int value) {
+ this.value = value;
+ }
+ }
+
+ protected DigitalSource m_upSource; // /< What makes the counter count up.
+ protected DigitalSource m_downSource; // /< What makes the counter count down.
+ private boolean m_allocatedUpSource;
+ private boolean m_allocatedDownSource;
+ private int m_counter; // /< The FPGA counter object.
+ private int m_index; // /< The index of this counter.
+ private PIDSourceType m_pidSource;
+ private double m_distancePerPulse; // distance of travel for each tick
+
+ /**
+ * Create an instance of a counter with the given mode.
+ */
+ public Counter(final Mode mode) {
+ ByteBuffer index = ByteBuffer.allocateDirect(4);
+ // set the byte order
+ index.order(ByteOrder.LITTLE_ENDIAN);
+ m_counter = CounterJNI.initializeCounter(mode.value, index.asIntBuffer());
+ m_index = index.asIntBuffer().get(0);
+
+ m_allocatedUpSource = false;
+ m_allocatedDownSource = false;
+ m_upSource = null;
+ m_downSource = null;
+
+ setMaxPeriod(0.5);
+
+ HAL.report(tResourceType.kResourceType_Counter, m_index + 1, mode.value + 1);
+ SendableRegistry.addLW(this, "Counter", m_index);
+ }
+
+ /**
+ * Create an instance of a counter where no sources are selected. Then they all must be selected
+ * by calling functions to specify the upsource and the downsource independently.
+ *
+ * <p>The counter will start counting immediately.
+ */
+ public Counter() {
+ this(Mode.kTwoPulse);
+ }
+
+ /**
+ * Create an instance of a counter from a Digital Input. This is used if an existing digital input
+ * is to be shared by multiple other objects such as encoders or if the Digital Source is not a
+ * DIO channel (such as an Analog Trigger)
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param source the digital source to count
+ */
+ public Counter(DigitalSource source) {
+ this();
+
+ requireNonNullParam(source, "source", "Counter");
+ setUpSource(source);
+ }
+
+ /**
+ * Create an instance of a Counter object. Create an up-Counter instance given a channel.
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param channel the DIO channel to use as the up source. 0-9 are on-board, 10-25 are on the MXP
+ */
+ public Counter(int channel) {
+ this();
+ setUpSource(channel);
+ }
+
+ /**
+ * Create an instance of a Counter object. Create an instance of a simple up-Counter given an
+ * analog trigger. Use the trigger state output from the analog trigger.
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param encodingType which edges to count
+ * @param upSource first source to count
+ * @param downSource second source for direction
+ * @param inverted true to invert the count
+ */
+ public Counter(EncodingType encodingType, DigitalSource upSource, DigitalSource downSource,
+ boolean inverted) {
+ this(Mode.kExternalDirection);
+
+ requireNonNullParam(encodingType, "encodingType", "Counter");
+ requireNonNullParam(upSource, "upSource", "Counter");
+ requireNonNullParam(downSource, "downSource", "Counter");
+
+ if (encodingType != EncodingType.k1X && encodingType != EncodingType.k2X) {
+ throw new IllegalArgumentException("Counters only support 1X and 2X quadrature decoding!");
+ }
+
+ setUpSource(upSource);
+ setDownSource(downSource);
+
+ if (encodingType == EncodingType.k1X) {
+ setUpSourceEdge(true, false);
+ CounterJNI.setCounterAverageSize(m_counter, 1);
+ } else {
+ setUpSourceEdge(true, true);
+ CounterJNI.setCounterAverageSize(m_counter, 2);
+ }
+
+ setDownSourceEdge(inverted, true);
+ }
+
+ /**
+ * Create an instance of a Counter object. Create an instance of a simple up-Counter given an
+ * analog trigger. Use the trigger state output from the analog trigger.
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param trigger the analog trigger to count
+ */
+ public Counter(AnalogTrigger trigger) {
+ this();
+
+ requireNonNullParam(trigger, "trigger", "Counter");
+
+ setUpSource(trigger.createOutput(AnalogTriggerType.kState));
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+
+ setUpdateWhenEmpty(true);
+
+ clearUpSource();
+ clearDownSource();
+
+ CounterJNI.freeCounter(m_counter);
+
+ m_upSource = null;
+ m_downSource = null;
+ m_counter = 0;
+ }
+
+ /**
+ * The counter's FPGA index.
+ *
+ * @return the Counter's FPGA index
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public int getFPGAIndex() {
+ return m_index;
+ }
+
+ /**
+ * Set the upsource for the counter as a digital input channel.
+ *
+ * @param channel the DIO channel to count 0-9 are on-board, 10-25 are on the MXP
+ */
+ public void setUpSource(int channel) {
+ setUpSource(new DigitalInput(channel));
+ m_allocatedUpSource = true;
+ SendableRegistry.addChild(this, m_upSource);
+ }
+
+ /**
+ * Set the source object that causes the counter to count up. Set the up counting DigitalSource.
+ *
+ * @param source the digital source to count
+ */
+ public void setUpSource(DigitalSource source) {
+ if (m_upSource != null && m_allocatedUpSource) {
+ m_upSource.close();
+ m_allocatedUpSource = false;
+ }
+ m_upSource = source;
+ CounterJNI.setCounterUpSource(m_counter, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting());
+ }
+
+ /**
+ * Set the up counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Up Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+ public void setUpSource(AnalogTrigger analogTrigger, AnalogTriggerType triggerType) {
+ requireNonNullParam(analogTrigger, "analogTrigger", "setUpSource");
+ requireNonNullParam(triggerType, "triggerType", "setUpSource");
+
+ setUpSource(analogTrigger.createOutput(triggerType));
+ m_allocatedUpSource = true;
+ }
+
+ /**
+ * Set the edge sensitivity on an up counting source. Set the up source to either detect rising
+ * edges or falling edges.
+ *
+ * @param risingEdge true to count rising edge
+ * @param fallingEdge true to count falling edge
+ */
+ public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
+ if (m_upSource == null) {
+ throw new IllegalStateException("Up Source must be set before setting the edge!");
+ }
+ CounterJNI.setCounterUpSourceEdge(m_counter, risingEdge, fallingEdge);
+ }
+
+ /**
+ * Disable the up counting source to the counter.
+ */
+ public void clearUpSource() {
+ if (m_upSource != null && m_allocatedUpSource) {
+ m_upSource.close();
+ m_allocatedUpSource = false;
+ }
+ m_upSource = null;
+
+ CounterJNI.clearCounterUpSource(m_counter);
+ }
+
+ /**
+ * Set the down counting source to be a digital input channel.
+ *
+ * @param channel the DIO channel to count 0-9 are on-board, 10-25 are on the MXP
+ */
+ public void setDownSource(int channel) {
+ setDownSource(new DigitalInput(channel));
+ m_allocatedDownSource = true;
+ SendableRegistry.addChild(this, m_downSource);
+ }
+
+ /**
+ * Set the source object that causes the counter to count down. Set the down counting
+ * DigitalSource.
+ *
+ * @param source the digital source to count
+ */
+ public void setDownSource(DigitalSource source) {
+ requireNonNull(source, "The Digital Source given was null");
+
+ if (m_downSource != null && m_allocatedDownSource) {
+ m_downSource.close();
+ m_allocatedDownSource = false;
+ }
+ CounterJNI.setCounterDownSource(m_counter, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting());
+ m_downSource = source;
+ }
+
+ /**
+ * Set the down counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Down Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+ public void setDownSource(AnalogTrigger analogTrigger, AnalogTriggerType triggerType) {
+ requireNonNullParam(analogTrigger, "analogTrigger", "setDownSource");
+ requireNonNullParam(triggerType, "analogTrigger", "setDownSource");
+
+ setDownSource(analogTrigger.createOutput(triggerType));
+ m_allocatedDownSource = true;
+ }
+
+ /**
+ * Set the edge sensitivity on a down counting source. Set the down source to either detect rising
+ * edges or falling edges.
+ *
+ * @param risingEdge true to count the rising edge
+ * @param fallingEdge true to count the falling edge
+ */
+ public void setDownSourceEdge(boolean risingEdge, boolean fallingEdge) {
+ requireNonNull(m_downSource, "Down Source must be set before setting the edge!");
+
+ CounterJNI.setCounterDownSourceEdge(m_counter, risingEdge, fallingEdge);
+ }
+
+ /**
+ * Disable the down counting source to the counter.
+ */
+ public void clearDownSource() {
+ if (m_downSource != null && m_allocatedDownSource) {
+ m_downSource.close();
+ m_allocatedDownSource = false;
+ }
+ m_downSource = null;
+
+ CounterJNI.clearCounterDownSource(m_counter);
+ }
+
+ /**
+ * Set standard up / down counting mode on this counter. Up and down counts are sourced
+ * independently from two inputs.
+ */
+ public void setUpDownCounterMode() {
+ CounterJNI.setCounterUpDownMode(m_counter);
+ }
+
+ /**
+ * Set external direction mode on this counter. Counts are sourced on the Up counter input. The
+ * Down counter input represents the direction to count.
+ */
+ public void setExternalDirectionMode() {
+ CounterJNI.setCounterExternalDirectionMode(m_counter);
+ }
+
+ /**
+ * Set Semi-period mode on this counter. Counts up on both rising and falling edges.
+ *
+ * @param highSemiPeriod true to count up on both rising and falling
+ */
+ public void setSemiPeriodMode(boolean highSemiPeriod) {
+ CounterJNI.setCounterSemiPeriodMode(m_counter, highSemiPeriod);
+ }
+
+ /**
+ * Configure the counter to count in up or down based on the length of the input pulse. This mode
+ * is most useful for direction sensitive gear tooth sensors.
+ *
+ * @param threshold The pulse length beyond which the counter counts the opposite direction. Units
+ * are seconds.
+ */
+ public void setPulseLengthMode(double threshold) {
+ CounterJNI.setCounterPulseLengthMode(m_counter, threshold);
+ }
+
+ /**
+ * Read the current counter value. Read the value at this instant. It may still be running, so it
+ * reflects the current value. Next time it is read, it might have a different value.
+ */
+ @Override
+ public int get() {
+ return CounterJNI.getCounter(m_counter);
+ }
+
+ /**
+ * Read the current scaled counter value. Read the value at this instant, scaled by the distance
+ * per pulse (defaults to 1).
+ *
+ * @return The distance since the last reset
+ */
+ public double getDistance() {
+ return get() * m_distancePerPulse;
+ }
+
+ /**
+ * Reset the Counter to zero. Set the counter value to zero. This doesn't effect the running state
+ * of the counter, just sets the current value to zero.
+ */
+ @Override
+ public void reset() {
+ CounterJNI.resetCounter(m_counter);
+ }
+
+ /**
+ * Set the maximum period where the device is still considered "moving". Sets the maximum period
+ * where the device is considered moving. This value is used to determine the "stopped" state of
+ * the counter using the GetStopped method.
+ *
+ * @param maxPeriod The maximum period where the counted device is considered moving in seconds.
+ */
+ @Override
+ public void setMaxPeriod(double maxPeriod) {
+ CounterJNI.setCounterMaxPeriod(m_counter, maxPeriod);
+ }
+
+ /**
+ * Select whether you want to continue updating the event timer output when there are no samples
+ * captured. The output of the event timer has a buffer of periods that are averaged and posted to
+ * a register on the FPGA. When the timer detects that the event source has stopped (based on the
+ * MaxPeriod) the buffer of samples to be averaged is emptied. If you enable the update when
+ * empty, you will be notified of the stopped source and the event time will report 0 samples. If
+ * you disable update when empty, the most recent average will remain on the output until a new
+ * sample is acquired. You will never see 0 samples output (except when there have been no events
+ * since an FPGA reset) and you will likely not see the stopped bit become true (since it is
+ * updated at the end of an average and there are no samples to average).
+ *
+ * @param enabled true to continue updating
+ */
+ public void setUpdateWhenEmpty(boolean enabled) {
+ CounterJNI.setCounterUpdateWhenEmpty(m_counter, enabled);
+ }
+
+ /**
+ * Determine if the clock is stopped. Determine if the clocked input is stopped based on the
+ * MaxPeriod value set using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
+ * device (and counter) are assumed to be stopped and it returns true.
+ *
+ * @return true if the most recent counter period exceeds the MaxPeriod value set by SetMaxPeriod.
+ */
+ @Override
+ public boolean getStopped() {
+ return CounterJNI.getCounterStopped(m_counter);
+ }
+
+ /**
+ * The last direction the counter value changed.
+ *
+ * @return The last direction the counter value changed.
+ */
+ @Override
+ public boolean getDirection() {
+ return CounterJNI.getCounterDirection(m_counter);
+ }
+
+ /**
+ * Set the Counter to return reversed sensing on the direction. This allows counters to change the
+ * direction they are counting in the case of 1X and 2X quadrature encoding only. Any other
+ * counter mode isn't supported.
+ *
+ * @param reverseDirection true if the value counted should be negated.
+ */
+ public void setReverseDirection(boolean reverseDirection) {
+ CounterJNI.setCounterReverseDirection(m_counter, reverseDirection);
+ }
+
+ /**
+ * Get the Period of the most recent count. Returns the time interval of the most recent count.
+ * This can be used for velocity calculations to determine shaft speed.
+ *
+ * @return The period of the last two pulses in units of seconds.
+ */
+ @Override
+ public double getPeriod() {
+ return CounterJNI.getCounterPeriod(m_counter);
+ }
+
+ /**
+ * Get the current rate of the Counter. Read the current rate of the counter accounting for the
+ * distance per pulse value. The default value for distance per pulse (1) yields units of pulses
+ * per second.
+ *
+ * @return The rate in units/sec
+ */
+ public double getRate() {
+ return m_distancePerPulse / getPeriod();
+ }
+
+ /**
+ * Set the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+ public void setSamplesToAverage(int samplesToAverage) {
+ CounterJNI.setCounterSamplesToAverage(m_counter, samplesToAverage);
+ }
+
+ /**
+ * Get the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+ public int getSamplesToAverage() {
+ return CounterJNI.getCounterSamplesToAverage(m_counter);
+ }
+
+ /**
+ * Set the distance per pulse for this counter. This sets the multiplier used to determine the
+ * distance driven based on the count value from the encoder. Set this value based on the Pulses
+ * per Revolution and factor in any gearing reductions. This distance can be in any units you
+ * like, linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+ */
+ public void setDistancePerPulse(double distancePerPulse) {
+ m_distancePerPulse = distancePerPulse;
+ }
+
+ /**
+ * Set which parameter of the encoder you are using as a process control variable. The counter
+ * class supports the rate and distance parameters.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ requireNonNullParam(pidSource, "pidSource", "setPIDSourceType");
+ if (pidSource != PIDSourceType.kDisplacement && pidSource != PIDSourceType.kRate) {
+ throw new IllegalArgumentException("PID Source parameter was not valid type: " + pidSource);
+ }
+
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ @Override
+ public double pidGet() {
+ switch (m_pidSource) {
+ case kDisplacement:
+ return getDistance();
+ case kRate:
+ return getRate();
+ default:
+ return 0.0;
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Counter");
+ builder.addDoubleProperty("Value", this::get, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
new file mode 100644
index 0000000..9d1d6b7
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for counting the number of ticks on a digital input channel. Encoders, Gear tooth
+ * sensors, and counters should all subclass this so it can be used to build more advanced classes
+ * for control and driving.
+ *
+ * <p>All counters will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public interface CounterBase {
+ /**
+ * The number of edges for the counterbase to increment or decrement on.
+ */
+ enum EncodingType {
+ /**
+ * Count only the rising edge.
+ */
+ k1X(0),
+ /**
+ * Count both the rising and falling edge.
+ */
+ k2X(1),
+ /**
+ * Count rising and falling on both channels.
+ */
+ k4X(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ EncodingType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Get the count.
+ *
+ * @return the count
+ */
+ int get();
+
+ /**
+ * Reset the count to zero.
+ */
+ void reset();
+
+ /**
+ * Get the time between the last two edges counted.
+ *
+ * @return the time between the last two ticks in seconds
+ */
+ double getPeriod();
+
+ /**
+ * Set the maximum time between edges to be considered stalled.
+ *
+ * @param maxPeriod the maximum period in seconds
+ */
+ void setMaxPeriod(double maxPeriod);
+
+ /**
+ * Determine if the counter is not moving.
+ *
+ * @return true if the counter has not changed for the max period
+ */
+ boolean getStopped();
+
+ /**
+ * Determine which direction the counter is going.
+ *
+ * @return true for one direction, false for the other
+ */
+ boolean getDirection();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
new file mode 100644
index 0000000..0feeebb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Digilent DMC 60 Speed Controller.
+ *
+ * <p>Note that the DMC uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the DMC 60 User Manual
+ * available from Digilent.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class DMC60 extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the DMC60 is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ public DMC60(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel() + 1);
+ SendableRegistry.setName(this, "DMC60", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
new file mode 100644
index 0000000..10155cc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.DigitalGlitchFilterJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class to enable glitch filtering on a set of digital inputs. This class will manage adding and
+ * removing digital inputs from a FPGA glitch filter. The filter lets the user configure the time
+ * that an input must remain high or low before it is classified as high or low.
+ */
+public class DigitalGlitchFilter implements Sendable, AutoCloseable {
+ /**
+ * Configures the Digital Glitch Filter to its default settings.
+ */
+ public DigitalGlitchFilter() {
+ synchronized (m_mutex) {
+ int index = 0;
+ while (m_filterAllocated[index] && index < m_filterAllocated.length) {
+ index++;
+ }
+ if (index != m_filterAllocated.length) {
+ m_channelIndex = index;
+ m_filterAllocated[index] = true;
+ HAL.report(tResourceType.kResourceType_DigitalGlitchFilter,
+ m_channelIndex + 1, 0);
+ SendableRegistry.addLW(this, "DigitalGlitchFilter", index);
+ }
+ }
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_channelIndex >= 0) {
+ synchronized (m_mutex) {
+ m_filterAllocated[m_channelIndex] = false;
+ }
+ m_channelIndex = -1;
+ }
+ }
+
+ private static void setFilter(DigitalSource input, int channelIndex) {
+ if (input != null) { // Counter might have just one input
+ // analog triggers are not supported for DigitalGlitchFilters
+ if (input.isAnalogTrigger()) {
+ throw new IllegalStateException("Analog Triggers not supported for DigitalGlitchFilters");
+ }
+ DigitalGlitchFilterJNI.setFilterSelect(input.getPortHandleForRouting(), channelIndex);
+
+ int selected = DigitalGlitchFilterJNI.getFilterSelect(input.getPortHandleForRouting());
+ if (selected != channelIndex) {
+ throw new IllegalStateException("DigitalGlitchFilterJNI.setFilterSelect("
+ + channelIndex + ") failed -> " + selected);
+ }
+ }
+ }
+
+ /**
+ * Assigns the DigitalSource to this glitch filter.
+ *
+ * @param input The DigitalSource to add.
+ */
+ public void add(DigitalSource input) {
+ setFilter(input, m_channelIndex + 1);
+ }
+
+ /**
+ * Assigns the Encoder to this glitch filter.
+ *
+ * @param input The Encoder to add.
+ */
+ public void add(Encoder input) {
+ add(input.m_aSource);
+ add(input.m_bSource);
+ }
+
+ /**
+ * Assigns the Counter to this glitch filter.
+ *
+ * @param input The Counter to add.
+ */
+ public void add(Counter input) {
+ add(input.m_upSource);
+ add(input.m_downSource);
+ }
+
+ /**
+ * Removes this filter from the given digital input.
+ *
+ * @param input The DigitalSource to stop filtering.
+ */
+ public void remove(DigitalSource input) {
+ setFilter(input, 0);
+ }
+
+ /**
+ * Removes this filter from the given Encoder.
+ *
+ * @param input the Encoder to stop filtering.
+ */
+ public void remove(Encoder input) {
+ remove(input.m_aSource);
+ remove(input.m_bSource);
+ }
+
+ /**
+ * Removes this filter from the given Counter.
+ *
+ * @param input The Counter to stop filtering.
+ */
+ public void remove(Counter input) {
+ remove(input.m_upSource);
+ remove(input.m_downSource);
+ }
+
+ /**
+ * Sets the number of FPGA cycles that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @param fpgaCycles The number of FPGA cycles.
+ */
+ public void setPeriodCycles(int fpgaCycles) {
+ DigitalGlitchFilterJNI.setFilterPeriod(m_channelIndex, fpgaCycles);
+ }
+
+ /**
+ * Sets the number of nanoseconds that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @param nanoseconds The number of nanoseconds.
+ */
+ public void setPeriodNanoSeconds(long nanoseconds) {
+ int fpgaCycles = (int) (nanoseconds * SensorUtil.kSystemClockTicksPerMicrosecond / 4
+ / 1000);
+ setPeriodCycles(fpgaCycles);
+ }
+
+ /**
+ * Gets the number of FPGA cycles that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @return The number of cycles.
+ */
+ public int getPeriodCycles() {
+ return DigitalGlitchFilterJNI.getFilterPeriod(m_channelIndex);
+ }
+
+ /**
+ * Gets the number of nanoseconds that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @return The number of nanoseconds.
+ */
+ public long getPeriodNanoSeconds() {
+ int fpgaCycles = getPeriodCycles();
+
+ return (long) fpgaCycles * 1000L
+ / (long) (SensorUtil.kSystemClockTicksPerMicrosecond / 4);
+ }
+
+ @Override
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public void initSendable(SendableBuilder builder) {
+ }
+
+ private int m_channelIndex = -1;
+ private static final Lock m_mutex = new ReentrantLock(true);
+ private static final boolean[] m_filterAllocated = new boolean[3];
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
new file mode 100644
index 0000000..7cbd1ed
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class to read a digital input. This class will read digital inputs and return the current value
+ * on the channel. Other devices such as encoders, gear tooth sensors, etc. that are implemented
+ * elsewhere will automatically allocate digital inputs and outputs as required. This class is only
+ * for devices like switches etc. that aren't implemented anywhere else.
+ */
+public class DigitalInput extends DigitalSource implements Sendable, AutoCloseable {
+ private final int m_channel;
+ private int m_handle;
+
+ /**
+ * Create an instance of a Digital Input class. Creates a digital input given a channel.
+ *
+ * @param channel the DIO channel for the digital input 0-9 are on-board, 10-25 are on the MXP
+ */
+ public DigitalInput(int channel) {
+ SensorUtil.checkDigitalChannel(channel);
+ m_channel = channel;
+
+ m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), true);
+
+ HAL.report(tResourceType.kResourceType_DigitalInput, channel + 1);
+ SendableRegistry.addLW(this, "DigitalInput", channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ SendableRegistry.remove(this);
+ if (m_interrupt != 0) {
+ cancelInterrupts();
+ }
+ DIOJNI.freeDIOPort(m_handle);
+ m_handle = 0;
+ }
+
+ /**
+ * Get the value from a digital input channel. Retrieve the value of a single digital input
+ * channel from the FPGA.
+ *
+ * @return the status of the digital input
+ */
+ public boolean get() {
+ return DIOJNI.getDIO(m_handle);
+ }
+
+ /**
+ * Get the channel of the digital input.
+ *
+ * @return The GPIO channel number that this object represents.
+ */
+ @Override
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Get the analog trigger type.
+ *
+ * @return false
+ */
+ @Override
+ public int getAnalogTriggerTypeForRouting() {
+ return 0;
+ }
+
+ /**
+ * Is this an analog trigger.
+ *
+ * @return true if this is an analog trigger
+ */
+ @Override
+ public boolean isAnalogTrigger() {
+ return false;
+ }
+
+ /**
+ * Get the HAL Port Handle.
+ *
+ * @return The HAL Handle to the specified source.
+ */
+ @Override
+ public int getPortHandleForRouting() {
+ return m_handle;
+ }
+
+ /**
+ * Indicates this input is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ public void setSimDevice(SimDevice device) {
+ DIOJNI.setDIOSimDevice(m_handle, device.getNativeHandle());
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Digital Input");
+ builder.addBooleanProperty("Value", this::get, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
new file mode 100644
index 0000000..ec1a44c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class to write digital outputs. This class will write digital outputs. Other devices that are
+ * implemented elsewhere will automatically allocate digital inputs and outputs as required.
+ */
+public class DigitalOutput extends DigitalSource implements Sendable, AutoCloseable {
+ private static final int invalidPwmGenerator = 0;
+ private int m_pwmGenerator = invalidPwmGenerator;
+
+ private final int m_channel;
+ private int m_handle;
+
+ /**
+ * Create an instance of a digital output. Create an instance of a digital output given a
+ * channel.
+ *
+ * @param channel the DIO channel to use for the digital output. 0-9 are on-board, 10-25 are on
+ * the MXP
+ */
+ public DigitalOutput(int channel) {
+ SensorUtil.checkDigitalChannel(channel);
+ m_channel = channel;
+
+ m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), false);
+
+ HAL.report(tResourceType.kResourceType_DigitalOutput, channel + 1);
+ SendableRegistry.addLW(this, "DigitalOutput", channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ SendableRegistry.remove(this);
+ // Disable the pwm only if we have allocated it
+ if (m_pwmGenerator != invalidPwmGenerator) {
+ disablePWM();
+ }
+ DIOJNI.freeDIOPort(m_handle);
+ m_handle = 0;
+ }
+
+ /**
+ * Set the value of a digital output.
+ *
+ * @param value true is on, off is false
+ */
+ public void set(boolean value) {
+ DIOJNI.setDIO(m_handle, (short) (value ? 1 : 0));
+ }
+
+ /**
+ * Gets the value being output from the Digital Output.
+ *
+ * @return the state of the digital output.
+ */
+ public boolean get() {
+ return DIOJNI.getDIO(m_handle);
+ }
+
+ /**
+ * Get the GPIO channel number that this object represents.
+ *
+ * @return The GPIO channel number.
+ */
+ @Override
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Generate a single pulse. There can only be a single pulse going at any time.
+ *
+ * @param pulseLength The length of the pulse.
+ */
+ public void pulse(final double pulseLength) {
+ DIOJNI.pulse(m_handle, pulseLength);
+ }
+
+ /**
+ * Determine if the pulse is still going. Determine if a previously started pulse is still going.
+ *
+ * @return true if pulsing
+ */
+ public boolean isPulsing() {
+ return DIOJNI.isPulsing(m_handle);
+ }
+
+ /**
+ * Change the PWM frequency of the PWM output on a Digital Output line.
+ *
+ * <p>The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic.
+ *
+ * <p>There is only one PWM frequency for all channels.
+ *
+ * @param rate The frequency to output all digital output PWM signals.
+ */
+ public void setPWMRate(double rate) {
+ DIOJNI.setDigitalPWMRate(rate);
+ }
+
+ /**
+ * Enable a PWM Output on this line.
+ *
+ * <p>Allocate one of the 6 DO PWM generator resources.
+ *
+ * <p>Supply the initial duty-cycle to output so as to avoid a glitch when first starting.
+ *
+ * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
+ * the higher the frequency of the PWM signal is.
+ *
+ * @param initialDutyCycle The duty-cycle to start generating. [0..1]
+ */
+ public void enablePWM(double initialDutyCycle) {
+ if (m_pwmGenerator != invalidPwmGenerator) {
+ return;
+ }
+ m_pwmGenerator = DIOJNI.allocateDigitalPWM();
+ DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle);
+ DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, m_channel);
+ }
+
+ /**
+ * Change this line from a PWM output back to a static Digital Output line.
+ *
+ * <p>Free up one of the 6 DO PWM generator resources that were in use.
+ */
+ public void disablePWM() {
+ if (m_pwmGenerator == invalidPwmGenerator) {
+ return;
+ }
+ // Disable the output by routing to a dead bit.
+ DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil.kDigitalChannels);
+ DIOJNI.freeDigitalPWM(m_pwmGenerator);
+ m_pwmGenerator = invalidPwmGenerator;
+ }
+
+ /**
+ * Change the duty-cycle that is being generated on the line.
+ *
+ * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
+ * the
+ * higher the frequency of the PWM signal is.
+ *
+ * @param dutyCycle The duty-cycle to change to. [0..1]
+ */
+ public void updateDutyCycle(double dutyCycle) {
+ if (m_pwmGenerator == invalidPwmGenerator) {
+ return;
+ }
+ DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle);
+ }
+
+ /**
+ * Indicates this input is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ public void setSimDevice(SimDevice device) {
+ DIOJNI.setDIOSimDevice(m_handle, device.getNativeHandle());
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Digital Output");
+ builder.addBooleanProperty("Value", this::get, this::set);
+ }
+
+ /**
+ * Is this an analog trigger.
+ *
+ * @return true if this is an analog trigger
+ */
+ @Override
+ public boolean isAnalogTrigger() {
+ return false;
+ }
+
+ /**
+ * Get the analog trigger type.
+ *
+ * @return false
+ */
+ @Override
+ public int getAnalogTriggerTypeForRouting() {
+ return 0;
+ }
+
+ /**
+ * Get the HAL Port Handle.
+ *
+ * @return The HAL Handle to the specified source.
+ */
+ @Override
+ public int getPortHandleForRouting() {
+ return m_handle;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
new file mode 100644
index 0000000..d31e191
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * DigitalSource Interface. The DigitalSource represents all the possible inputs for a counter or a
+ * quadrature encoder. The source may be either a digital input or an analog input. If the caller
+ * just provides a channel, then a digital input will be constructed and freed when finished for the
+ * source. The source can either be a digital input or analog trigger but not both.
+ */
+public abstract class DigitalSource extends InterruptableSensorBase {
+ public abstract boolean isAnalogTrigger();
+
+ public abstract int getChannel();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
new file mode 100644
index 0000000..d612d24
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SolenoidJNI;
+import edu.wpi.first.hal.util.UncleanStatusException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the PCM.
+ *
+ * <p>The DoubleSolenoid class is typically used for pneumatics solenoids that have two positions
+ * controlled by two separate channels.
+ */
+public class DoubleSolenoid extends SolenoidBase implements Sendable, AutoCloseable {
+ /**
+ * Possible values for a DoubleSolenoid.
+ */
+ public enum Value {
+ kOff,
+ kForward,
+ kReverse
+ }
+
+ private byte m_forwardMask; // The mask for the forward channel.
+ private byte m_reverseMask; // The mask for the reverse channel.
+ private int m_forwardHandle;
+ private int m_reverseHandle;
+
+ /**
+ * Constructor. Uses the default PCM ID (defaults to 0).
+ *
+ * @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) {
+ this(SensorUtil.getDefaultSolenoidModule(), forwardChannel, reverseChannel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The module number of the solenoid module to use.
+ * @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);
+
+ SensorUtil.checkSolenoidModule(m_moduleNumber);
+ SensorUtil.checkSolenoidChannel(forwardChannel);
+ SensorUtil.checkSolenoidChannel(reverseChannel);
+
+ int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) forwardChannel);
+ m_forwardHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+
+ try {
+ portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) reverseChannel);
+ m_reverseHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+ } catch (UncleanStatusException ex) {
+ // free the forward handle on exception, then rethrow
+ SolenoidJNI.freeSolenoidPort(m_forwardHandle);
+ m_forwardHandle = 0;
+ m_reverseHandle = 0;
+ throw ex;
+ }
+
+ m_forwardMask = (byte) (1 << forwardChannel);
+ m_reverseMask = (byte) (1 << reverseChannel);
+
+ HAL.report(tResourceType.kResourceType_Solenoid, forwardChannel + 1,
+ m_moduleNumber + 1);
+ HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel + 1,
+ m_moduleNumber + 1);
+ SendableRegistry.addLW(this, "DoubleSolenoid", m_moduleNumber, forwardChannel);
+ }
+
+ @Override
+ public synchronized void close() {
+ SendableRegistry.remove(this);
+ SolenoidJNI.freeSolenoidPort(m_forwardHandle);
+ SolenoidJNI.freeSolenoidPort(m_reverseHandle);
+ }
+
+ /**
+ * Set the value of a solenoid.
+ *
+ * @param value The value to set (Off, Forward, Reverse)
+ */
+ public void set(final Value value) {
+ boolean forward = false;
+ boolean reverse = false;
+
+ switch (value) {
+ case kOff:
+ forward = false;
+ reverse = false;
+ break;
+ case kForward:
+ forward = true;
+ reverse = false;
+ break;
+ case kReverse:
+ forward = false;
+ reverse = true;
+ break;
+ default:
+ throw new AssertionError("Illegal value: " + value);
+
+ }
+
+ SolenoidJNI.setSolenoid(m_forwardHandle, forward);
+ SolenoidJNI.setSolenoid(m_reverseHandle, reverse);
+ }
+
+ /**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+ public Value get() {
+ boolean valueForward = SolenoidJNI.getSolenoid(m_forwardHandle);
+ boolean valueReverse = SolenoidJNI.getSolenoid(m_reverseHandle);
+
+ if (valueForward) {
+ return Value.kForward;
+ }
+ if (valueReverse) {
+ return Value.kReverse;
+ }
+ return Value.kOff;
+ }
+
+ /**
+ * Check if the forward solenoid is blacklisted. If a solenoid is shorted, it is added to the
+ * blacklist and disabled until power cycle, or until faults are cleared.
+ *
+ * @return If solenoid is disabled due to short.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public boolean isFwdSolenoidBlackListed() {
+ int blackList = getPCMSolenoidBlackList();
+ return (blackList & m_forwardMask) != 0;
+ }
+
+ /**
+ * Check if the reverse solenoid is blacklisted. If a solenoid is shorted, it is added to the
+ * blacklist and disabled until power cycle, or until faults are cleared.
+ *
+ * @return If solenoid is disabled due to short.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public boolean isRevSolenoidBlackListed() {
+ int blackList = getPCMSolenoidBlackList();
+ return (blackList & m_reverseMask) != 0;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Double Solenoid");
+ builder.setActuator(true);
+ builder.setSafeState(() -> set(Value.kOff));
+ builder.addStringProperty("Value", () -> get().name().substring(1), value -> {
+ if ("Forward".equals(value)) {
+ set(Value.kForward);
+ } else if ("Reverse".equals(value)) {
+ set(Value.kReverse);
+ } else {
+ set(Value.kOff);
+ }
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
new file mode 100644
index 0000000..997fe2b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -0,0 +1,1150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * Provide access to the network communication data to / from the Driver Station.
+ */
+@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.ExcessiveClassLength",
+ "PMD.ExcessivePublicCount", "PMD.GodClass", "PMD.TooManyFields",
+ "PMD.TooManyMethods"})
+public class DriverStation {
+ /**
+ * Number of Joystick Ports.
+ */
+ public static final int kJoystickPorts = 6;
+
+ private static class HALJoystickButtons {
+ public int m_buttons;
+ public byte m_count;
+ }
+
+ private static class HALJoystickAxes {
+ public float[] m_axes;
+ public short m_count;
+
+ HALJoystickAxes(int count) {
+ m_axes = new float[count];
+ }
+ }
+
+ private static class HALJoystickPOVs {
+ public short[] m_povs;
+ public short m_count;
+
+ HALJoystickPOVs(int count) {
+ m_povs = new short[count];
+ for (int i = 0; i < count; i++) {
+ m_povs[i] = -1;
+ }
+ }
+ }
+
+ /**
+ * The robot alliance that the robot is a part of.
+ */
+ public enum Alliance {
+ Red, Blue, Invalid
+ }
+
+ public enum MatchType {
+ None, Practice, Qualification, Elimination
+ }
+
+ private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
+ private double m_nextMessageTime;
+
+ private static class DriverStationTask implements Runnable {
+ private final DriverStation m_ds;
+
+ DriverStationTask(DriverStation ds) {
+ m_ds = ds;
+ }
+
+ @Override
+ public void run() {
+ m_ds.run();
+ }
+ } /* DriverStationTask */
+
+ private static class MatchDataSender {
+ @SuppressWarnings("MemberName")
+ NetworkTable table;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry typeMetadata;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry gameSpecificMessage;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry eventName;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry matchNumber;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry replayNumber;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry matchType;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry alliance;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry station;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry controlWord;
+
+ MatchDataSender() {
+ table = NetworkTableInstance.getDefault().getTable("FMSInfo");
+ typeMetadata = table.getEntry(".type");
+ typeMetadata.forceSetString("FMSInfo");
+ gameSpecificMessage = table.getEntry("GameSpecificMessage");
+ gameSpecificMessage.forceSetString("");
+ eventName = table.getEntry("EventName");
+ eventName.forceSetString("");
+ matchNumber = table.getEntry("MatchNumber");
+ matchNumber.forceSetDouble(0);
+ replayNumber = table.getEntry("ReplayNumber");
+ replayNumber.forceSetDouble(0);
+ matchType = table.getEntry("MatchType");
+ matchType.forceSetDouble(0);
+ alliance = table.getEntry("IsRedAlliance");
+ alliance.forceSetBoolean(true);
+ station = table.getEntry("StationNumber");
+ station.forceSetDouble(1);
+ controlWord = table.getEntry("FMSControlData");
+ controlWord.forceSetDouble(0);
+ }
+ }
+
+ private static DriverStation instance = new DriverStation();
+
+ // Joystick User Data
+ private HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
+ private HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
+ private HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
+ private MatchInfoData m_matchInfo = new MatchInfoData();
+
+ // Joystick Cached Data
+ private HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
+ private HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
+ private HALJoystickButtons[] m_joystickButtonsCache = new HALJoystickButtons[kJoystickPorts];
+ private MatchInfoData m_matchInfoCache = new MatchInfoData();
+
+ // Joystick button rising/falling edge flags
+ private int[] m_joystickButtonsPressed = new int[kJoystickPorts];
+ private int[] m_joystickButtonsReleased = new int[kJoystickPorts];
+
+ // preallocated byte buffer for button count
+ private final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
+
+ private final MatchDataSender m_matchDataSender;
+
+ // Internal Driver Station thread
+ @SuppressWarnings("PMD.SingularField")
+ private final Thread m_thread;
+ private volatile boolean m_threadKeepAlive = true;
+
+ private final ReentrantLock m_cacheDataMutex = new ReentrantLock();
+
+ private final Lock m_waitForDataMutex;
+ private final Condition m_waitForDataCond;
+ private int m_waitForDataCount;
+
+ // Robot state status variables
+ private boolean m_userInDisabled;
+ private boolean m_userInAutonomous;
+ private boolean m_userInTeleop;
+ private boolean m_userInTest;
+
+ // Control word variables
+ private final Object m_controlWordMutex;
+ private final ControlWord m_controlWordCache;
+ private long m_lastControlWordUpdate;
+
+ /**
+ * Gets an instance of the DriverStation.
+ *
+ * @return The DriverStation.
+ */
+ public static DriverStation getInstance() {
+ return DriverStation.instance;
+ }
+
+ /**
+ * DriverStation constructor.
+ *
+ * <p>The single DriverStation instance is created statically with the instance static member
+ * variable.
+ */
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ private DriverStation() {
+ HAL.initialize(500, 0);
+ m_waitForDataCount = 0;
+ m_waitForDataMutex = new ReentrantLock();
+ m_waitForDataCond = m_waitForDataMutex.newCondition();
+
+ for (int i = 0; i < kJoystickPorts; i++) {
+ m_joystickButtons[i] = new HALJoystickButtons();
+ m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
+ m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
+
+ m_joystickButtonsCache[i] = new HALJoystickButtons();
+ m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
+ m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
+ }
+
+ m_controlWordMutex = new Object();
+ m_controlWordCache = new ControlWord();
+ m_lastControlWordUpdate = 0;
+
+ m_matchDataSender = new MatchDataSender();
+
+ m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
+ m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
+
+ m_thread.start();
+ }
+
+ /**
+ * Kill the thread.
+ */
+ public void release() {
+ m_threadKeepAlive = false;
+ }
+
+ /**
+ * Report error to Driver Station. Optionally appends Stack trace
+ * to error message.
+ *
+ * @param printTrace If true, append stack trace to error string
+ */
+ public static void reportError(String error, boolean printTrace) {
+ reportErrorImpl(true, 1, error, printTrace);
+ }
+
+ /**
+ * Report error to Driver Station. Appends provided stack trace
+ * to error message.
+ *
+ * @param stackTrace The stack trace to append
+ */
+ public static void reportError(String error, StackTraceElement[] stackTrace) {
+ reportErrorImpl(true, 1, error, stackTrace);
+ }
+
+ /**
+ * Report warning to Driver Station. Optionally appends Stack
+ * trace to warning message.
+ *
+ * @param printTrace If true, append stack trace to warning string
+ */
+ public static void reportWarning(String error, boolean printTrace) {
+ reportErrorImpl(false, 1, error, printTrace);
+ }
+
+ /**
+ * Report warning to Driver Station. Appends provided stack
+ * trace to warning message.
+ *
+ * @param stackTrace The stack trace to append
+ */
+ public static void reportWarning(String error, StackTraceElement[] stackTrace) {
+ reportErrorImpl(false, 1, error, stackTrace);
+ }
+
+ private static void reportErrorImpl(boolean isError, int code, String error, boolean
+ printTrace) {
+ reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3);
+ }
+
+ private static void reportErrorImpl(boolean isError, int code, String error,
+ StackTraceElement[] stackTrace) {
+ reportErrorImpl(isError, code, error, true, stackTrace, 0);
+ }
+
+ private static void reportErrorImpl(boolean isError, int code, String error,
+ boolean printTrace, StackTraceElement[] stackTrace, int stackTraceFirst) {
+ String locString;
+ if (stackTrace.length >= stackTraceFirst + 1) {
+ locString = stackTrace[stackTraceFirst].toString();
+ } else {
+ locString = "";
+ }
+ StringBuilder traceString = new StringBuilder();
+ if (printTrace) {
+ boolean haveLoc = false;
+ for (int i = stackTraceFirst; i < stackTrace.length; i++) {
+ String loc = stackTrace[i].toString();
+ traceString.append("\tat ").append(loc).append('\n');
+ // get first user function
+ if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
+ locString = loc;
+ haveLoc = true;
+ }
+ }
+ }
+ HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
+ }
+
+ /**
+ * The state of one joystick button. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return The state of the joystick button.
+ */
+ public boolean getStickButton(final int stick, final int button) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+ if (button <= 0) {
+ reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+ return false;
+ }
+ m_cacheDataMutex.lock();
+ try {
+ if (button > m_joystickButtons[stick].m_count) {
+ // Unlock early so error printing isn't locked.
+ m_cacheDataMutex.unlock();
+ reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+
+ return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0;
+ } finally {
+ if (m_cacheDataMutex.isHeldByCurrentThread()) {
+ m_cacheDataMutex.unlock();
+ }
+ }
+ }
+
+ /**
+ * Whether one joystick button was pressed since the last check. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return Whether the joystick button was pressed since the last check.
+ */
+ boolean getStickButtonPressed(final int stick, final int button) {
+ if (button <= 0) {
+ reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+ return false;
+ }
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+ boolean error = false;
+ boolean retVal = false;
+ synchronized (m_cacheDataMutex) {
+ if (button > m_joystickButtons[stick].m_count) {
+ error = true;
+ retVal = false;
+ } else {
+ // If button was pressed, clear flag and return true
+ if ((m_joystickButtonsPressed[stick] & 1 << (button - 1)) != 0) {
+ m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
+ retVal = true;
+ } else {
+ retVal = false;
+ }
+ }
+ }
+ if (error) {
+ reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+ return retVal;
+ }
+
+ /**
+ * Whether one joystick button was released since the last check. Button indexes
+ * begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return Whether the joystick button was released since the last check.
+ */
+ boolean getStickButtonReleased(final int stick, final int button) {
+ if (button <= 0) {
+ reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+ return false;
+ }
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+ boolean error = false;
+ boolean retVal = false;
+ synchronized (m_cacheDataMutex) {
+ if (button > m_joystickButtons[stick].m_count) {
+ error = true;
+ retVal = false;
+ } else {
+ // If button was released, clear flag and return true
+ if ((m_joystickButtonsReleased[stick] & 1 << (button - 1)) != 0) {
+ m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
+ retVal = true;
+ } else {
+ retVal = false;
+ }
+ }
+ }
+ if (error) {
+ reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+ return retVal;
+ }
+
+ /**
+ * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected
+ * to the specified port.
+ *
+ * @param stick The joystick to read.
+ * @param axis The analog axis value to read from the joystick.
+ * @return The value of the axis on the joystick.
+ */
+ public double getStickAxis(int stick, int axis) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+ if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
+ throw new IllegalArgumentException("Joystick axis is out of range");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ if (axis >= m_joystickAxes[stick].m_count) {
+ // Unlock early so error printing isn't locked.
+ m_cacheDataMutex.unlock();
+ reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
+ + " not available, check if controller is plugged in");
+ return 0.0;
+ }
+
+ return m_joystickAxes[stick].m_axes[axis];
+ } finally {
+ if (m_cacheDataMutex.isHeldByCurrentThread()) {
+ m_cacheDataMutex.unlock();
+ }
+ }
+ }
+
+ /**
+ * Get the state of a POV on the joystick.
+ *
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+ public int getStickPOV(int stick, int pov) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+ if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
+ throw new IllegalArgumentException("Joystick POV is out of range");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ if (pov >= m_joystickPOVs[stick].m_count) {
+ // Unlock early so error printing isn't locked.
+ m_cacheDataMutex.unlock();
+ reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
+ + " not available, check if controller is plugged in");
+ return -1;
+ }
+ } finally {
+ if (m_cacheDataMutex.isHeldByCurrentThread()) {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ return m_joystickPOVs[stick].m_povs[pov];
+ }
+
+ /**
+ * The state of the buttons on the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+ public int getStickButtons(final int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickButtons[stick].m_buttons;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the number of axes on a given joystick port.
+ *
+ * @param stick The joystick port number
+ * @return The number of axes on the indicated joystick
+ */
+ public int getStickAxisCount(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickAxes[stick].m_count;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the number of POVs on a given joystick port.
+ *
+ * @param stick The joystick port number
+ * @return The number of POVs on the indicated joystick
+ */
+ public int getStickPOVCount(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickPOVs[stick].m_count;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Gets the number of buttons on a joystick.
+ *
+ * @param stick The joystick port number
+ * @return The number of buttons on the indicated joystick
+ */
+ public int getStickButtonCount(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickButtons[stick].m_count;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Gets the value of isXbox on a joystick.
+ *
+ * @param stick The joystick port number
+ * @return A boolean that returns the value of isXbox
+ */
+ public boolean getJoystickIsXbox(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickIsXbox((byte) stick) == 1;
+ }
+
+ /**
+ * Gets the value of type on a joystick.
+ *
+ * @param stick The joystick port number
+ * @return The value of type
+ */
+ public int getJoystickType(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickType((byte) stick);
+ }
+
+ /**
+ * Gets the name of the joystick at a port.
+ *
+ * @param stick The joystick port number
+ * @return The value of name
+ */
+ public String getJoystickName(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickName((byte) stick);
+ }
+
+ /**
+ * Returns the types of Axes on a given joystick port.
+ *
+ * @param stick The joystick port number
+ * @param axis The target axis
+ * @return What type of axis the axis is reporting to be
+ */
+ public int getJoystickAxisType(int stick, int axis) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickAxisType((byte) stick, (byte) axis);
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be enabled.
+ *
+ * @return True if the robot is enabled, false otherwise.
+ */
+ public boolean isEnabled() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be disabled.
+ *
+ * @return True if the robot should be disabled, false otherwise.
+ */
+ public boolean isDisabled() {
+ return !isEnabled();
+ }
+
+ /**
+ * Gets a value indicating whether the Robot is e-stopped.
+ *
+ * @return True if the robot is e-stopped, false otherwise.
+ */
+ public boolean isEStopped() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getEStop();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be running in
+ * autonomous mode.
+ *
+ * @return True if autonomous mode should be enabled, false otherwise.
+ */
+ public boolean isAutonomous() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getAutonomous();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be running in
+ * operator-controlled mode.
+ *
+ * @return True if operator-controlled mode should be enabled, false otherwise.
+ */
+ public boolean isOperatorControl() {
+ return !(isAutonomous() || isTest());
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be running in test
+ * mode.
+ *
+ * @return True if test mode should be enabled, false otherwise.
+ */
+ public boolean isTest() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getTest();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station is attached.
+ *
+ * @return True if Driver Station is attached, false otherwise.
+ */
+ public boolean isDSAttached() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getDSAttached();
+ }
+ }
+
+ /**
+ * Gets if a new control packet from the driver station arrived since the last time this function
+ * was called.
+ *
+ * @return True if the control data has been updated since the last call.
+ */
+ public boolean isNewControlData() {
+ return HAL.isNewControlData();
+ }
+
+ /**
+ * Gets if the driver station attached to a Field Management System.
+ *
+ * @return true if the robot is competing on a field being controlled by a Field Management System
+ */
+ public boolean isFMSAttached() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getFMSAttached();
+ }
+ }
+
+ /**
+ * Get the game specific message.
+ *
+ * @return the game specific message
+ */
+ public String getGameSpecificMessage() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.gameSpecificMessage;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the event name.
+ *
+ * @return the event name
+ */
+ public String getEventName() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.eventName;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the match type.
+ *
+ * @return the match type
+ */
+ public MatchType getMatchType() {
+ int matchType;
+ m_cacheDataMutex.lock();
+ try {
+ matchType = m_matchInfo.matchType;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ switch (matchType) {
+ case 1:
+ return MatchType.Practice;
+ case 2:
+ return MatchType.Qualification;
+ case 3:
+ return MatchType.Elimination;
+ default:
+ return MatchType.None;
+ }
+ }
+
+ /**
+ * Get the match number.
+ *
+ * @return the match number
+ */
+ public int getMatchNumber() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.matchNumber;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the replay number.
+ *
+ * @return the replay number
+ */
+ public int getReplayNumber() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.replayNumber;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the current alliance from the FMS.
+ *
+ * @return the current alliance
+ */
+ public Alliance getAlliance() {
+ AllianceStationID allianceStationID = HAL.getAllianceStation();
+ if (allianceStationID == null) {
+ return Alliance.Invalid;
+ }
+
+ switch (allianceStationID) {
+ case Red1:
+ case Red2:
+ case Red3:
+ return Alliance.Red;
+
+ case Blue1:
+ case Blue2:
+ case Blue3:
+ return Alliance.Blue;
+
+ default:
+ return Alliance.Invalid;
+ }
+ }
+
+ /**
+ * Gets the location of the team's driver station controls.
+ *
+ * @return the location of the team's driver station controls: 1, 2, or 3
+ */
+ public int getLocation() {
+ AllianceStationID allianceStationID = HAL.getAllianceStation();
+ if (allianceStationID == null) {
+ return 0;
+ }
+ switch (allianceStationID) {
+ case Red1:
+ case Blue1:
+ return 1;
+
+ case Red2:
+ case Blue2:
+ return 2;
+
+ case Blue3:
+ case Red3:
+ return 3;
+
+ default:
+ return 0;
+ }
+ }
+
+ /**
+ * Wait for new data from the driver station.
+ */
+ public void waitForData() {
+ waitForData(0);
+ }
+
+ /**
+ * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
+ * only.
+ *
+ * @param timeout The maximum time in seconds to wait.
+ * @return true if there is new data, otherwise false
+ */
+ public boolean waitForData(double timeout) {
+ long startTime = RobotController.getFPGATime();
+ long timeoutMicros = (long) (timeout * 1000000);
+ m_waitForDataMutex.lock();
+ try {
+ int currentCount = m_waitForDataCount;
+ while (m_waitForDataCount == currentCount) {
+ if (timeout > 0) {
+ long now = RobotController.getFPGATime();
+ if (now < startTime + timeoutMicros) {
+ // We still have time to wait
+ boolean signaled = m_waitForDataCond.await(startTime + timeoutMicros - now,
+ TimeUnit.MICROSECONDS);
+ if (!signaled) {
+ // Return false if a timeout happened
+ return false;
+ }
+ } else {
+ // Time has elapsed.
+ return false;
+ }
+ } else {
+ m_waitForDataCond.await();
+ }
+ }
+ // Return true if we have received a proper signal
+ return true;
+ } catch (InterruptedException ex) {
+ // return false on a thread interrupt
+ return false;
+ } finally {
+ m_waitForDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Return the approximate match time. The FMS does not send an official match time to the robots,
+ * but does send an approximate match time. The value will count down the time remaining in the
+ * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
+ * dispute ref calls or guarantee that a function will trigger before the match ends) The
+ * Practice Match function of the DS approximates the behaviour seen on the field.
+ *
+ * @return Time remaining in current match period (auto or teleop) in seconds
+ */
+ public double getMatchTime() {
+ return HAL.getMatchTime();
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting disabled code; if false, leaving disabled code
+ */
+ @SuppressWarnings("MethodName")
+ public void InDisabled(boolean entering) {
+ m_userInDisabled = entering;
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting autonomous code; if false, leaving autonomous code
+ */
+ @SuppressWarnings("MethodName")
+ public void InAutonomous(boolean entering) {
+ m_userInAutonomous = entering;
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting teleop code; if false, leaving teleop code
+ */
+ @SuppressWarnings("MethodName")
+ public void InOperatorControl(boolean entering) {
+ m_userInTeleop = entering;
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting test code; if false, leaving test code
+ */
+ @SuppressWarnings("MethodName")
+ public void InTest(boolean entering) {
+ m_userInTest = entering;
+ }
+
+ private void sendMatchData() {
+ AllianceStationID alliance = HAL.getAllianceStation();
+ boolean isRedAlliance = false;
+ int stationNumber = 1;
+ switch (alliance) {
+ case Blue1:
+ isRedAlliance = false;
+ stationNumber = 1;
+ break;
+ case Blue2:
+ isRedAlliance = false;
+ stationNumber = 2;
+ break;
+ case Blue3:
+ isRedAlliance = false;
+ stationNumber = 3;
+ break;
+ case Red1:
+ isRedAlliance = true;
+ stationNumber = 1;
+ break;
+ case Red2:
+ isRedAlliance = true;
+ stationNumber = 2;
+ break;
+ default:
+ isRedAlliance = true;
+ stationNumber = 3;
+ break;
+ }
+
+
+ String eventName;
+ String gameSpecificMessage;
+ int matchNumber;
+ int replayNumber;
+ int matchType;
+ synchronized (m_cacheDataMutex) {
+ eventName = m_matchInfo.eventName;
+ gameSpecificMessage = m_matchInfo.gameSpecificMessage;
+ matchNumber = m_matchInfo.matchNumber;
+ replayNumber = m_matchInfo.replayNumber;
+ matchType = m_matchInfo.matchType;
+ }
+
+ m_matchDataSender.alliance.setBoolean(isRedAlliance);
+ m_matchDataSender.station.setDouble(stationNumber);
+ m_matchDataSender.eventName.setString(eventName);
+ m_matchDataSender.gameSpecificMessage.setString(gameSpecificMessage);
+ m_matchDataSender.matchNumber.setDouble(matchNumber);
+ m_matchDataSender.replayNumber.setDouble(replayNumber);
+ m_matchDataSender.matchType.setDouble(matchType);
+ m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord());
+ }
+
+ /**
+ * Forces waitForData() to return immediately.
+ */
+ public void wakeupWaitForData() {
+ m_waitForDataMutex.lock();
+ m_waitForDataCount++;
+ m_waitForDataCond.signalAll();
+ m_waitForDataMutex.unlock();
+ }
+
+ /**
+ * Copy data from the DS task for the user. If no new data exists, it will just be returned,
+ * otherwise the data will be copied from the DS polling loop.
+ */
+ protected void getData() {
+ // Get the status of all of the joysticks
+ for (byte stick = 0; stick < kJoystickPorts; stick++) {
+ m_joystickAxesCache[stick].m_count =
+ HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
+ m_joystickPOVsCache[stick].m_count =
+ HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
+ m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
+ m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
+ }
+
+ HAL.getMatchInfo(m_matchInfoCache);
+
+ // Force a control word update, to make sure the data is the newest.
+ updateControlWord(true);
+
+ // lock joystick mutex to swap cache data
+ m_cacheDataMutex.lock();
+ try {
+ for (int i = 0; i < kJoystickPorts; i++) {
+ // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+ m_joystickButtonsPressed[i] |=
+ ~m_joystickButtons[i].m_buttons & m_joystickButtonsCache[i].m_buttons;
+
+ // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+ m_joystickButtonsReleased[i] |=
+ m_joystickButtons[i].m_buttons & ~m_joystickButtonsCache[i].m_buttons;
+ }
+
+ // move cache to actual data
+ HALJoystickAxes[] currentAxes = m_joystickAxes;
+ m_joystickAxes = m_joystickAxesCache;
+ m_joystickAxesCache = currentAxes;
+
+ HALJoystickButtons[] currentButtons = m_joystickButtons;
+ m_joystickButtons = m_joystickButtonsCache;
+ m_joystickButtonsCache = currentButtons;
+
+ HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
+ m_joystickPOVs = m_joystickPOVsCache;
+ m_joystickPOVsCache = currentPOVs;
+
+ MatchInfoData currentInfo = m_matchInfo;
+ m_matchInfo = m_matchInfoCache;
+ m_matchInfoCache = currentInfo;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+
+ wakeupWaitForData();
+ sendMatchData();
+ }
+
+ /**
+ * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+ * the DS.
+ */
+ private void reportJoystickUnpluggedError(String message) {
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ reportError(message, false);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+ }
+
+ /**
+ * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+ * the DS.
+ */
+ private void reportJoystickUnpluggedWarning(String message) {
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ reportWarning(message, false);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+ }
+
+ /**
+ * Provides the service routine for the DS polling m_thread.
+ */
+ private void run() {
+ int safetyCounter = 0;
+ while (m_threadKeepAlive) {
+ HAL.waitForDSData();
+ getData();
+
+ if (isDisabled()) {
+ safetyCounter = 0;
+ }
+
+ safetyCounter++;
+ if (safetyCounter >= 4) {
+ MotorSafety.checkMotors();
+ safetyCounter = 0;
+ }
+ if (m_userInDisabled) {
+ HAL.observeUserProgramDisabled();
+ }
+ if (m_userInAutonomous) {
+ HAL.observeUserProgramAutonomous();
+ }
+ if (m_userInTeleop) {
+ HAL.observeUserProgramTeleop();
+ }
+ if (m_userInTest) {
+ HAL.observeUserProgramTest();
+ }
+ }
+ }
+
+ /**
+ * Updates the data in the control word cache. Updates if the force parameter is set, or if
+ * 50ms have passed since the last update.
+ *
+ * @param force True to force an update to the cache, otherwise update if 50ms have passed.
+ */
+ private void updateControlWord(boolean force) {
+ long now = System.currentTimeMillis();
+ synchronized (m_controlWordMutex) {
+ if (now - m_lastControlWordUpdate > 50 || force) {
+ HAL.getControlWord(m_controlWordCache);
+ m_lastControlWordUpdate = now;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
new file mode 100644
index 0000000..0834fad
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DutyCycleJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to low
+ * in that frequency. There are 8 of these in the roboRIO, and they can be
+ * attached to any {@link DigitalSource}.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in order
+ * to implement rollover checking.
+ *
+ */
+public class DutyCycle implements Sendable, AutoCloseable {
+ // Explicitly package private
+ final int m_handle;
+
+ private final DigitalSource m_source;
+
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p>This class does not own the inputted source.
+ *
+ * @param digitalSource The DigitalSource to use.
+ */
+ public DutyCycle(DigitalSource digitalSource) {
+ m_handle = DutyCycleJNI.initialize(digitalSource.getPortHandleForRouting(),
+ digitalSource.getAnalogTriggerTypeForRouting());
+
+ m_source = digitalSource;
+ int index = getFPGAIndex();
+ HAL.report(tResourceType.kResourceType_DutyCycle, index + 1);
+ SendableRegistry.addLW(this, "Duty Cycle", index);
+ }
+
+ /**
+ * Close the DutyCycle and free all resources.
+ */
+ @Override
+ public void close() {
+ DutyCycleJNI.free(m_handle);
+ }
+
+ /**
+ * Get the frequency of the duty cycle signal.
+ *
+ * @return frequency in Hertz
+ */
+ public int getFrequency() {
+ return DutyCycleJNI.getFrequency(m_handle);
+ }
+
+ /**
+ * Get the output ratio of the duty cycle signal.
+ *
+ * <p>0 means always low, 1 means always high.
+ *
+ * @return output ratio between 0 and 1
+ */
+ public double getOutput() {
+ return DutyCycleJNI.getOutput(m_handle);
+ }
+
+ /**
+ * Get the raw output ratio of the duty cycle signal.
+ *
+ * <p>0 means always low, an output equal to getOutputScaleFactor() means always
+ * high.
+ *
+ * @return output ratio in raw units
+ */
+ public int getOutputRaw() {
+ return DutyCycleJNI.getOutputRaw(m_handle);
+ }
+
+ /**
+ * Get the scale factor of the output.
+ *
+ * <p>An output equal to this value is always high, and then linearly scales down
+ * to 0. Divide the result of getOutputRaw by this in order to get the
+ * percentage between 0 and 1.
+ *
+ * @return the output scale factor
+ */
+ public int getOutputScaleFactor() {
+ return DutyCycleJNI.getOutputScaleFactor(m_handle);
+ }
+
+ /**
+ * Get the FPGA index for the DutyCycle.
+ *
+ * @return the FPGA index
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public final int getFPGAIndex() {
+ return DutyCycleJNI.getFPGAIndex(m_handle);
+ }
+
+ public int getSourceChannel() {
+ return m_source.getChannel();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Duty Cycle");
+ builder.addDoubleProperty("Frequency", this::getFrequency, null);
+ builder.addDoubleProperty("Output", this::getOutput, null);
+
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
new file mode 100644
index 0000000..b2040f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
@@ -0,0 +1,229 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with
+ * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ * Encoder.
+ */
+public class DutyCycleEncoder implements Sendable, AutoCloseable {
+ private final DutyCycle m_dutyCycle;
+ private boolean m_ownsDutyCycle;
+ private AnalogTrigger m_analogTrigger;
+ private Counter m_counter;
+ private int m_frequencyThreshold = 100;
+ private double m_positionOffset;
+ private double m_distancePerRotation = 1.0;
+ private double m_lastPosition;
+
+ protected SimDevice m_simDevice;
+ protected SimDouble m_simPosition;
+ protected SimBoolean m_simIsConnected;
+
+ /**
+ * Construct a new DutyCycleEncoder on a specific channel.
+ *
+ * @param channel the channel to attach to
+ */
+ public DutyCycleEncoder(int channel) {
+ m_dutyCycle = new DutyCycle(new DigitalInput(channel));
+ init();
+ }
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ public DutyCycleEncoder(DutyCycle dutyCycle) {
+ m_dutyCycle = dutyCycle;
+ init();
+ }
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ public DutyCycleEncoder(DigitalSource source) {
+ m_ownsDutyCycle = true;
+ m_dutyCycle = new DutyCycle(source);
+ init();
+ }
+
+ private void init() {
+ m_analogTrigger = new AnalogTrigger(m_dutyCycle);
+ m_counter = new Counter();
+
+ m_simDevice = SimDevice.create("DutyCycleEncoder", m_dutyCycle.getFPGAIndex());
+
+ if (m_simDevice != null) {
+ m_simPosition = m_simDevice.createDouble("Position", false, 0.0);
+ m_simIsConnected = m_simDevice.createBoolean("Connected", false, true);
+ }
+
+ m_analogTrigger.setLimitsDutyCycle(0.25, 0.75);
+ m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
+ m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
+
+ SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
+ }
+
+ /**
+ * Get the encoder value since the last reset.
+ *
+ * <p>This is reported in rotations since the last reset.
+ *
+ * @return the encoder value in rotations
+ */
+ public double get() {
+ if (m_simPosition != null) {
+ return m_simPosition.get();
+ }
+
+ // As the values are not atomic, keep trying until we get 2 reads of the same
+ // value
+ // If we don't within 10 attempts, error
+ for (int i = 0; i < 10; i++) {
+ double counter = m_counter.get();
+ double pos = m_dutyCycle.getOutput();
+ double counter2 = m_counter.get();
+ double pos2 = m_dutyCycle.getOutput();
+ if (counter == counter2 && pos == pos2) {
+ double position = counter + pos - m_positionOffset;
+ m_lastPosition = position;
+ return position;
+ }
+ }
+
+ DriverStation.reportWarning(
+ "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
+ return m_lastPosition;
+ }
+
+ /**
+ * Get the offset of position relative to the last reset.
+ *
+ * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute
+ * position relative to the last reset. This could potentially be negative,
+ * which needs to be accounted for.
+ *
+ * @return the position offset
+ */
+ public double getPositionOffset() {
+ return m_positionOffset;
+ }
+
+ /**
+ * Set the distance per rotation of the encoder. This sets the multiplier used
+ * to determine the distance driven based on the rotation value from the
+ * encoder. Set this value based on the how far the mechanism travels in 1
+ * rotation of the encoder, and factor in gearing reductions following the
+ * encoder shaft. This distance can be in any units you like, linear or angular.
+ *
+ * @param distancePerRotation the distance per rotation of the encoder
+ */
+ public void setDistancePerRotation(double distancePerRotation) {
+ m_distancePerRotation = distancePerRotation;
+ }
+
+ /**
+ * Get the distance per rotation for this encoder.
+ *
+ * @return The scale factor that will be used to convert rotation to useful
+ * units.
+ */
+ public double getDistancePerRotation() {
+ return m_distancePerRotation;
+ }
+
+ /**
+ * Get the distance the sensor has driven since the last reset as scaled by the
+ * value from {@link #setDistancePerRotation(double)}.
+ *
+ * @return The distance driven since the last reset
+ */
+ public double getDistance() {
+ return get() * getDistancePerRotation();
+ }
+
+ /**
+ * Get the frequency in Hz of the duty cycle signal from the encoder.
+ *
+ * @return duty cycle frequency in Hz
+ */
+ public int getFrequency() {
+ return m_dutyCycle.getFrequency();
+ }
+
+ /**
+ * Reset the Encoder distance to zero.
+ */
+ public void reset() {
+ m_counter.reset();
+ m_positionOffset = m_dutyCycle.getOutput();
+ }
+
+ /**
+ * Get if the sensor is connected
+ *
+ * <p>This uses the duty cycle frequency to determine if the sensor is connected.
+ * By default, a value of 100 Hz is used as the threshold, and this value can be
+ * changed with {@link #setConnectedFrequencyThreshold(int)}.
+ *
+ * @return true if the sensor is connected
+ */
+ public boolean isConnected() {
+ if (m_simIsConnected != null) {
+ return m_simIsConnected.get();
+ }
+ return getFrequency() > m_frequencyThreshold;
+ }
+
+ /**
+ * Change the frequency threshold for detecting connection used by
+ * {@link #isConnected()}.
+ *
+ * @param frequency the minimum frequency in Hz.
+ */
+ public void setConnectedFrequencyThreshold(int frequency) {
+ if (frequency < 0) {
+ frequency = 0;
+ }
+
+ m_frequencyThreshold = frequency;
+ }
+
+ @Override
+ public void close() {
+ m_counter.close();
+ m_analogTrigger.close();
+ if (m_ownsDutyCycle) {
+ m_dutyCycle.close();
+ }
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("AbsoluteEncoder");
+ builder.addDoubleProperty("Distance", this::getDistance, null);
+ builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
+ builder.addBooleanProperty("Is Connected", this::isConnected, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
new file mode 100644
index 0000000..125f769
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -0,0 +1,586 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.EncoderJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * Class to read quadrature encoders.
+ *
+ * <p>Quadrature encoders are devices that count shaft rotation and can sense direction. The output
+ * of the Encoder class is an integer that can count either up or down, and can go negative for
+ * reverse direction counting. When creating Encoders, a direction can be supplied that inverts the
+ * sense of the output to make code more readable if the encoder is mounted such that forward
+ * movement generates negative values. Quadrature encoders have two digital outputs, an A Channel
+ * and a B Channel, that are out of phase with each other for direction sensing.
+ *
+ * <p>All encoders will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable {
+ public enum IndexingType {
+ kResetWhileHigh(0), kResetWhileLow(1), kResetOnFallingEdge(2), kResetOnRisingEdge(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ IndexingType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * The a source.
+ */
+ @SuppressWarnings("MemberName")
+ protected DigitalSource m_aSource; // the A phase of the quad encoder
+ /**
+ * The b source.
+ */
+ @SuppressWarnings("MemberName")
+ protected DigitalSource m_bSource; // the B phase of the quad encoder
+ /**
+ * The index source.
+ */
+ protected DigitalSource m_indexSource; // Index on some encoders
+ private boolean m_allocatedA;
+ private boolean m_allocatedB;
+ private boolean m_allocatedI;
+ private PIDSourceType m_pidSource;
+
+ private int m_encoder; // the HAL encoder object
+
+
+ /**
+ * Common initialization code for Encoders. This code allocates resources for Encoders and is
+ * common to all constructors.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param reverseDirection If true, counts down instead of up (this is all relative)
+ */
+ private void initEncoder(boolean reverseDirection, final EncodingType type) {
+ m_encoder = EncoderJNI.initializeEncoder(m_aSource.getPortHandleForRouting(),
+ m_aSource.getAnalogTriggerTypeForRouting(), m_bSource.getPortHandleForRouting(),
+ m_bSource.getAnalogTriggerTypeForRouting(), reverseDirection, type.value);
+
+ m_pidSource = PIDSourceType.kDisplacement;
+
+ int fpgaIndex = getFPGAIndex();
+ HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
+ SendableRegistry.addLW(this, "Encoder", fpgaIndex);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+ * @param channelB The b channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(final int channelA, final int channelB, boolean reverseDirection) {
+ this(channelA, channelB, reverseDirection, EncodingType.k4X);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ */
+ public Encoder(final int channelA, final int channelB) {
+ this(channelA, channelB, false);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts
+ * will be 4x the encoder spec'd value since all rising and falling edges
+ * are counted. If 1X or 2X are selected then a m_counter object will be
+ * used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+ public Encoder(final int channelA, final int channelB, boolean reverseDirection,
+ final EncodingType encodingType) {
+ requireNonNullParam(encodingType, "encodingType", "Encoder");
+
+ m_allocatedA = true;
+ m_allocatedB = true;
+ m_allocatedI = false;
+ m_aSource = new DigitalInput(channelA);
+ m_bSource = new DigitalInput(channelB);
+ SendableRegistry.addChild(this, m_aSource);
+ SendableRegistry.addChild(this, m_bSource);
+ initEncoder(reverseDirection, encodingType);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels. Using an index pulse forces 4x
+ * encoding
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ * @param indexChannel The index channel digital input channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(final int channelA, final int channelB, final int indexChannel,
+ boolean reverseDirection) {
+ this(channelA, channelB, reverseDirection);
+ m_allocatedI = true;
+ m_indexSource = new DigitalInput(indexChannel);
+ SendableRegistry.addChild(this, m_indexSource);
+ setIndexSource(m_indexSource);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels. Using an index pulse forces 4x
+ * encoding
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ * @param indexChannel The index channel digital input channel.
+ */
+ public Encoder(final int channelA, final int channelB, final int indexChannel) {
+ this(channelA, channelB, indexChannel, false);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b 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.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) {
+ this(sourceA, sourceB, reverseDirection, EncodingType.k4X);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b 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.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB) {
+ this(sourceA, sourceB, false);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b 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.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts
+ * will be 4x the encoder spec'd value since all rising and falling edges
+ * are counted. If 1X or 2X are selected then a m_counter object will be
+ * used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection,
+ final EncodingType encodingType) {
+ requireNonNullParam(sourceA, "sourceA", "Encoder");
+ requireNonNullParam(sourceB, "sourceB", "Encoder");
+ requireNonNullParam(encodingType, "encodingType", "Encoder");
+
+ m_allocatedA = false;
+ m_allocatedB = false;
+ m_allocatedI = false;
+ m_aSource = sourceA;
+ m_bSource = sourceB;
+ initEncoder(reverseDirection, encodingType);
+ }
+
+ /**
+ * 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.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param indexSource the source that should be used for the index channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource,
+ boolean reverseDirection) {
+ this(sourceA, sourceB, reverseDirection);
+ m_allocatedI = false;
+ m_indexSource = indexSource;
+ setIndexSource(indexSource);
+ }
+
+ /**
+ * 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.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param indexSource the source that should be used for the index channel.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) {
+ this(sourceA, sourceB, indexSource, false);
+ }
+
+ /**
+ * Get the FPGA index of the encoder.
+ *
+ * @return The Encoder's FPGA index.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public int getFPGAIndex() {
+ return EncoderJNI.getEncoderFPGAIndex(m_encoder);
+ }
+
+ /**
+ * Used to divide raw edge counts down to spec'd counts.
+ *
+ * @return The encoding scale factor 1x, 2x, or 4x, per the requested encoding type.
+ */
+ public int getEncodingScale() {
+ return EncoderJNI.getEncoderEncodingScale(m_encoder);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_aSource != null && m_allocatedA) {
+ m_aSource.close();
+ m_allocatedA = false;
+ }
+ if (m_bSource != null && m_allocatedB) {
+ m_bSource.close();
+ m_allocatedB = false;
+ }
+ if (m_indexSource != null && m_allocatedI) {
+ m_indexSource.close();
+ m_allocatedI = false;
+ }
+
+ m_aSource = null;
+ m_bSource = null;
+ m_indexSource = null;
+ EncoderJNI.freeEncoder(m_encoder);
+ m_encoder = 0;
+ }
+
+ /**
+ * Gets the raw value from the encoder. The raw value is the actual count unscaled by the 1x, 2x,
+ * or 4x scale factor.
+ *
+ * @return Current raw count from the encoder
+ */
+ public int getRaw() {
+ return EncoderJNI.getEncoderRaw(m_encoder);
+ }
+
+ /**
+ * Gets the current count. Returns the current count on the Encoder. This method compensates for
+ * the decoding type.
+ *
+ * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
+ */
+ @Override
+ public int get() {
+ return EncoderJNI.getEncoder(m_encoder);
+ }
+
+ /**
+ * Reset the Encoder distance to zero. Resets the current count to zero on the encoder.
+ */
+ @Override
+ public void reset() {
+ EncoderJNI.resetEncoder(m_encoder);
+ }
+
+ /**
+ * Returns the period of the most recent pulse. Returns the period of the most recent Encoder
+ * pulse in seconds. This method compensates for the decoding type.
+ *
+ * <p><b>Warning:</b> This returns unscaled periods. Use getRate() for rates that are scaled using
+ * the value from setDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ * @deprecated Use getRate() in favor of this method.
+ */
+ @Override
+ @Deprecated
+ public double getPeriod() {
+ return EncoderJNI.getEncoderPeriod(m_encoder);
+ }
+
+ /**
+ * Sets the maximum period for stopped detection. Sets the value that represents the maximum
+ * period of the Encoder before it will assume that the attached device is stopped. This timeout
+ * allows users to determine if the wheels or other shaft has stopped rotating. This method
+ * compensates for the decoding type.
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the FPGA will report
+ * the device stopped. This is expressed in seconds.
+ */
+ @Override
+ public void setMaxPeriod(double maxPeriod) {
+ EncoderJNI.setEncoderMaxPeriod(m_encoder, maxPeriod);
+ }
+
+ /**
+ * Determine if the encoder is stopped. Using the MaxPeriod value, a boolean is returned that is
+ * true if the encoder is considered stopped and false if it is still moving. A stopped encoder is
+ * one where the most recent pulse width exceeds the MaxPeriod.
+ *
+ * @return True if the encoder is considered stopped.
+ */
+ @Override
+ public boolean getStopped() {
+ return EncoderJNI.getEncoderStopped(m_encoder);
+ }
+
+ /**
+ * The last direction the encoder value changed.
+ *
+ * @return The last direction the encoder value changed.
+ */
+ @Override
+ public boolean getDirection() {
+ return EncoderJNI.getEncoderDirection(m_encoder);
+ }
+
+ /**
+ * Get the distance the robot has driven since the last reset as scaled by the value from {@link
+ * #setDistancePerPulse(double)}.
+ *
+ * @return The distance driven since the last reset
+ */
+ public double getDistance() {
+ return EncoderJNI.getEncoderDistance(m_encoder);
+ }
+
+ /**
+ * Get the current rate of the encoder. Units are distance per second as scaled by the value from
+ * setDistancePerPulse().
+ *
+ * @return The current rate of the encoder.
+ */
+ public double getRate() {
+ return EncoderJNI.getEncoderRate(m_encoder);
+ }
+
+ /**
+ * Set the minimum rate of the device before the hardware reports it stopped.
+ *
+ * @param minRate The minimum rate. The units are in distance per second as scaled by the value
+ * from setDistancePerPulse().
+ */
+ public void setMinRate(double minRate) {
+ EncoderJNI.setEncoderMinRate(m_encoder, minRate);
+ }
+
+ /**
+ * Set the distance per pulse for this encoder. This sets the multiplier used to determine the
+ * distance driven based on the count value from the encoder. Do not include the decoding type in
+ * this scale. The library already compensates for the decoding type. Set this value based on the
+ * encoder's rated Pulses per Revolution and factor in gearing reductions following the encoder
+ * shaft. This distance can be in any units you like, linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+ */
+ public void setDistancePerPulse(double distancePerPulse) {
+ EncoderJNI.setEncoderDistancePerPulse(m_encoder, distancePerPulse);
+ }
+
+ /**
+ * Get the distance per pulse for this encoder.
+ *
+ * @return The scale factor that will be used to convert pulses to useful units.
+ */
+ public double getDistancePerPulse() {
+ return EncoderJNI.getEncoderDistancePerPulse(m_encoder);
+ }
+
+ /**
+ * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so
+ * that it could count in the correct software direction regardless of the mounting.
+ *
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+ public void setReverseDirection(boolean reverseDirection) {
+ EncoderJNI.setEncoderReverseDirection(m_encoder, reverseDirection);
+ }
+
+ /**
+ * Set the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+ public void setSamplesToAverage(int samplesToAverage) {
+ EncoderJNI.setEncoderSamplesToAverage(m_encoder, samplesToAverage);
+ }
+
+ /**
+ * Get the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+ public int getSamplesToAverage() {
+ return EncoderJNI.getEncoderSamplesToAverage(m_encoder);
+ }
+
+ /**
+ * Set which parameter of the encoder you are using as a process control variable. The encoder
+ * class supports the rate and distance parameters.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Implement the PIDSource interface.
+ *
+ * @return The current value of the selected source parameter.
+ */
+ @Override
+ public double pidGet() {
+ switch (m_pidSource) {
+ case kDisplacement:
+ return getDistance();
+ case kRate:
+ return getRate();
+ default:
+ return 0.0;
+ }
+ }
+
+ /**
+ * 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) {
+ setIndexSource(channel, IndexingType.kResetOnRisingEdge);
+ }
+
+ /**
+ * 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) {
+ setIndexSource(source, IndexingType.kResetOnRisingEdge);
+ }
+
+ /**
+ * 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) {
+ if (m_allocatedI) {
+ throw new AllocationException("Digital Input for Indexing already allocated");
+ }
+ m_indexSource = new DigitalInput(channel);
+ m_allocatedI = true;
+ SendableRegistry.addChild(this, m_indexSource);
+ setIndexSource(m_indexSource, type);
+ }
+
+ /**
+ * 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) {
+ EncoderJNI.setEncoderIndexSource(m_encoder, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting(), type.value);
+ }
+
+ /**
+ * Indicates this input is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ public void setSimDevice(SimDevice device) {
+ EncoderJNI.setEncoderSimDevice(m_encoder, device.getNativeHandle());
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) {
+ builder.setSmartDashboardType("Quadrature Encoder");
+ } else {
+ builder.setSmartDashboardType("Encoder");
+ }
+
+ builder.addDoubleProperty("Speed", this::getRate, null);
+ builder.addDoubleProperty("Distance", this::getDistance, null);
+ builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
new file mode 100644
index 0000000..cfefb6a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.File;
+
+/**
+ * Class for interacting with the Filesystem, particularly, interacting with
+ * FRC-related paths on the system, such as the launch and deploy directories.
+ *
+ * <p>This class is primarily used for obtaining resources in src/main/deploy, and
+ * the RoboRIO path /home/lvuser in a simulation-compatible way.</p>
+ */
+public final class Filesystem {
+ private Filesystem() { }
+
+ /**
+ * Obtains the current working path that the program was launched with.
+ * This is analogous to the `pwd` command on unix.
+ *
+ * @return The current working directory (launch directory)
+ */
+ public static File getLaunchDirectory() {
+ return new File(System.getProperty("user.dir")).getAbsoluteFile();
+ }
+
+ /**
+ * Obtains the operating directory of the program. On the roboRIO, this is
+ * /home/lvuser. In simulation, it is where the simulation was launched from
+ * (`pwd`).
+ *
+ * @return The operating directory
+ */
+ public static File getOperatingDirectory() {
+ if (RobotBase.isReal()) {
+ return new File("/home/lvuser");
+ } else {
+ return getLaunchDirectory();
+ }
+ }
+
+ /**
+ * Obtains the deploy directory of the program, which is the remote location
+ * src/main/deploy is deployed to by default. On the roboRIO, this is
+ * /home/lvuser/deploy. In simulation, it is where the simulation was launched
+ * from, in the subdirectory "deploy" (`pwd`/deploy).
+ *
+ * @return The deploy directory
+ */
+ public static File getDeployDirectory() {
+ return new File(getOperatingDirectory(), "deploy");
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
new file mode 100644
index 0000000..41f9d09
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Alias for counter class. Implement the gear tooth sensor supplied by FIRST. Currently there is no
+ * reverse sensing on the gear tooth sensor, but in future versions we might implement the necessary
+ * timing in the FPGA to sense direction.
+ *
+ * @deprecated The only sensor this works with is no longer available and no teams use it according
+ * to FMS usage reporting.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public class GearTooth extends Counter {
+ private static final double kGearToothThreshold = 55e-6;
+
+ /**
+ * Common code called by the constructors.
+ */
+ public void enableDirectionSensing(boolean directionSensitive) {
+ if (directionSensitive) {
+ setPulseLengthMode(kGearToothThreshold);
+ }
+ }
+
+ /**
+ * Construct a GearTooth sensor given a channel.
+ *
+ * <p>No direction sensing is assumed.
+ *
+ * @param channel The GPIO channel that the sensor is connected to.
+ */
+ public GearTooth(final int channel) {
+ this(channel, false);
+ }
+
+ /**
+ * Construct a GearTooth sensor given a channel.
+ *
+ * @param channel The DIO channel that the sensor is connected to. 0-9 are on-board,
+ * 10-25 are on the MXP port
+ * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
+ * direction.
+ */
+ public GearTooth(final int channel, boolean directionSensitive) {
+ super(channel);
+ enableDirectionSensing(directionSensitive);
+ if (directionSensitive) {
+ HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0, "D");
+ } else {
+ HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0);
+ }
+ SendableRegistry.setName(this, "GearTooth", channel);
+ }
+
+ /**
+ * Construct a GearTooth sensor given a digital input. This should be used when sharing digital
+ * inputs.
+ *
+ * @param source An existing DigitalSource object (such as a DigitalInput)
+ * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
+ * direction.
+ */
+ public GearTooth(DigitalSource source, boolean directionSensitive) {
+ super(source);
+ enableDirectionSensing(directionSensitive);
+ if (directionSensitive) {
+ HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0, "D");
+ } else {
+ HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0);
+ }
+ SendableRegistry.setName(this, "GearTooth", source.getChannel());
+ }
+
+ /**
+ * Construct a GearTooth sensor given a digital input. This should be used when sharing digital
+ * inputs.
+ *
+ * <p>No direction sensing is assumed.
+ *
+ * @param source An object that fully describes the input that the sensor is connected to.
+ */
+ public GearTooth(DigitalSource source) {
+ this(source, false);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ super.initSendable(builder);
+ builder.setSmartDashboardType("Gear Tooth");
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
new file mode 100644
index 0000000..0f884b9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
@@ -0,0 +1,286 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import edu.wpi.first.hal.HAL;
+
+/**
+ * GenericHID Interface.
+ */
+public abstract class GenericHID {
+ /**
+ * Represents a rumble output on the JoyStick.
+ */
+ public enum RumbleType {
+ kLeftRumble, kRightRumble
+ }
+
+ public enum HIDType {
+ kUnknown(-1),
+ kXInputUnknown(0),
+ kXInputGamepad(1),
+ kXInputWheel(2),
+ kXInputArcadeStick(3),
+ kXInputFlightStick(4),
+ kXInputDancePad(5),
+ kXInputGuitar(6),
+ kXInputGuitar2(7),
+ kXInputDrumKit(8),
+ kXInputGuitar3(11),
+ kXInputArcadePad(19),
+ kHIDJoystick(20),
+ kHIDGamepad(21),
+ kHIDDriving(22),
+ kHIDFlight(23),
+ kHID1stPerson(24);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private static final Map<Integer, HIDType> map = new HashMap<>();
+
+ HIDType(int value) {
+ this.value = value;
+ }
+
+ static {
+ for (HIDType hidType : HIDType.values()) {
+ map.put(hidType.value, hidType);
+ }
+ }
+
+ public static HIDType of(int value) {
+ return map.get(value);
+ }
+ }
+
+ /**
+ * Which hand the Human Interface Device is associated with.
+ */
+ public enum Hand {
+ kLeft(0), kRight(1);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Hand(int value) {
+ this.value = value;
+ }
+ }
+
+ private DriverStation m_ds;
+ private final int m_port;
+ private int m_outputs;
+ private short m_leftRumble;
+ private short m_rightRumble;
+
+ public GenericHID(int port) {
+ m_ds = DriverStation.getInstance();
+ m_port = port;
+ }
+
+ /**
+ * Get the x position of the HID.
+ *
+ * @return the x position of the HID
+ */
+ public final double getX() {
+ return getX(Hand.kRight);
+ }
+
+ /**
+ * Get the x position of HID.
+ *
+ * @param hand which hand, left or right
+ * @return the x position
+ */
+ public abstract double getX(Hand hand);
+
+ /**
+ * Get the y position of the HID.
+ *
+ * @return the y position
+ */
+ public final double getY() {
+ return getY(Hand.kRight);
+ }
+
+ /**
+ * Get the y position of the HID.
+ *
+ * @param hand which hand, left or right
+ * @return the y position
+ */
+ public abstract double getY(Hand hand);
+
+ /**
+ * Get the button value (starting at button 1).
+ *
+ * <p>The buttons are returned in a single 16 bit value with one bit representing the state of
+ * each button. The appropriate button is returned as a boolean value.
+ *
+ * @param button The button number to be read (starting at 1)
+ * @return The state of the button.
+ */
+ public boolean getRawButton(int button) {
+ return m_ds.getStickButton(m_port, (byte) button);
+ }
+
+ /**
+ * Whether the button was pressed since the last check. Button indexes begin at
+ * 1.
+ *
+ * @param button The button index, beginning at 1.
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getRawButtonPressed(int button) {
+ return m_ds.getStickButtonPressed(m_port, (byte) button);
+ }
+
+ /**
+ * Whether the button was released since the last check. Button indexes begin at
+ * 1.
+ *
+ * @param button The button index, beginning at 1.
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getRawButtonReleased(int button) {
+ return m_ds.getStickButtonReleased(m_port, button);
+ }
+
+ /**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+ public double getRawAxis(int axis) {
+ return m_ds.getStickAxis(m_port, axis);
+ }
+
+ /**
+ * Get the angle in degrees of a POV on the HID.
+ *
+ * <p>The POV angles start at 0 in the up direction, and increase clockwise (eg right is 90,
+ * upper-left is 315).
+ *
+ * @param pov The index of the POV to read (starting at 0)
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+ public int getPOV(int pov) {
+ return m_ds.getStickPOV(m_port, pov);
+ }
+
+ public int getPOV() {
+ return getPOV(0);
+ }
+
+ /**
+ * Get the number of axes for the HID.
+ *
+ * @return the number of axis for the current HID
+ */
+ public int getAxisCount() {
+ return m_ds.getStickAxisCount(m_port);
+ }
+
+ /**
+ * For the current HID, return the number of POVs.
+ */
+ public int getPOVCount() {
+ return m_ds.getStickPOVCount(m_port);
+ }
+
+ /**
+ * For the current HID, return the number of buttons.
+ */
+ public int getButtonCount() {
+ return m_ds.getStickButtonCount(m_port);
+ }
+
+ /**
+ * Get the type of the HID.
+ *
+ * @return the type of the HID.
+ */
+ public HIDType getType() {
+ return HIDType.of(m_ds.getJoystickType(m_port));
+ }
+
+ /**
+ * Get the name of the HID.
+ *
+ * @return the name of the HID.
+ */
+ public String getName() {
+ return m_ds.getJoystickName(m_port);
+ }
+
+ /**
+ * Get the axis type of a joystick axis.
+ *
+ * @return the axis type of a joystick axis.
+ */
+ public int getAxisType(int axis) {
+ return m_ds.getJoystickAxisType(m_port, axis);
+ }
+
+ /**
+ * Get the port number of the HID.
+ *
+ * @return The port number of the HID.
+ */
+ public int getPort() {
+ return m_port;
+ }
+
+ /**
+ * Set a single HID output value for the HID.
+ *
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value The value to set the output to
+ */
+ public void setOutput(int outputNumber, boolean value) {
+ m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
+ HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+ }
+
+ /**
+ * Set all HID output values for the HID.
+ *
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+ public void setOutputs(int value) {
+ m_outputs = value;
+ HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+ }
+
+ /**
+ * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and
+ * right rumble.
+ *
+ * @param type Which rumble value to set
+ * @param value The normalized value (0 to 1) to set the rumble to
+ */
+ public void setRumble(RumbleType type, double value) {
+ if (value < 0) {
+ value = 0;
+ } else if (value > 1) {
+ value = 1;
+ }
+ if (type == RumbleType.kLeftRumble) {
+ m_leftRumble = (short) (value * 65535);
+ } else {
+ m_rightRumble = (short) (value * 65535);
+ }
+ HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
new file mode 100644
index 0000000..21330ce
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * GyroBase is the common base class for Gyro implementations such as AnalogGyro.
+ */
+public abstract class GyroBase implements Gyro, PIDSource, Sendable {
+ private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Set which parameter of the gyro you are using as a process control variable. The Gyro class
+ * supports the rate and displacement parameters
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on
+ * the set PIDSourceType
+ *
+ * @return the output according to the gyro
+ */
+ @Override
+ public double pidGet() {
+ switch (m_pidSource) {
+ case kRate:
+ return getRate();
+ case kDisplacement:
+ return getAngle();
+ default:
+ return 0.0;
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Gyro");
+ builder.addDoubleProperty("Value", this::getAngle, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
new file mode 100644
index 0000000..ca8374a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
@@ -0,0 +1,374 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.I2CJNI;
+import edu.wpi.first.hal.util.BoundaryException;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * I2C bus interface class.
+ *
+ * <p>This class is intended to be used by sensor (and other I2C device) drivers. It probably should
+ * not be used directly.
+ */
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+public class I2C implements AutoCloseable {
+ public enum Port {
+ kOnboard(0), kMXP(1);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Port(int value) {
+ this.value = value;
+ }
+ }
+
+ private final int m_port;
+ private final int m_deviceAddress;
+
+ /**
+ * Constructor.
+ *
+ * @param port The I2C port the device is connected to.
+ * @param deviceAddress The address of the device on the I2C bus.
+ */
+ public I2C(Port port, int deviceAddress) {
+ m_port = port.value;
+ m_deviceAddress = deviceAddress;
+
+ I2CJNI.i2CInitialize((byte) port.value);
+
+ HAL.report(tResourceType.kResourceType_I2C, deviceAddress);
+ }
+
+ @Override
+ public void close() {
+ I2CJNI.i2CClose(m_port);
+ }
+
+ /**
+ * Generic transaction.
+ *
+ * <p>This is a lower-level interface to the I2C hardware giving you more control over each
+ * transaction. If you intend to write multiple bytes in the same transaction and do not
+ * plan to receive anything back, use writeBulk() instead. Calling this with a receiveSize
+ * of 0 will result in an error.
+ *
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param sendSize Number of bytes to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param receiveSize Number of bytes to read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean transaction(byte[] dataToSend, int sendSize,
+ byte[] dataReceived, int receiveSize) {
+ if (dataToSend.length < sendSize) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
+ }
+ if (dataReceived.length < receiveSize) {
+ throw new IllegalArgumentException(
+ "dataReceived is too small, must be at least " + receiveSize);
+ }
+ return I2CJNI.i2CTransactionB(m_port, (byte) m_deviceAddress, dataToSend,
+ (byte) sendSize, dataReceived, (byte) receiveSize) < 0;
+ }
+
+ /**
+ * Generic transaction.
+ *
+ * <p>This is a lower-level interface to the I2C hardware giving you more control over each
+ * transaction.
+ *
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param sendSize Number of bytes to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param receiveSize Number of bytes to read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "ByteBufferBackingArray"})
+ public synchronized boolean transaction(ByteBuffer dataToSend, int sendSize,
+ ByteBuffer dataReceived, int receiveSize) {
+ if (dataToSend.hasArray() && dataReceived.hasArray()) {
+ return transaction(dataToSend.array(), sendSize, dataReceived.array(), receiveSize);
+ }
+ if (!dataToSend.isDirect()) {
+ throw new IllegalArgumentException("dataToSend must be a direct buffer");
+ }
+ if (dataToSend.capacity() < sendSize) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
+ }
+ if (!dataReceived.isDirect()) {
+ throw new IllegalArgumentException("dataReceived must be a direct buffer");
+ }
+ if (dataReceived.capacity() < receiveSize) {
+ throw new IllegalArgumentException(
+ "dataReceived is too small, must be at least " + receiveSize);
+ }
+
+ return I2CJNI.i2CTransaction(m_port, (byte) m_deviceAddress, dataToSend,
+ (byte) sendSize, dataReceived, (byte) receiveSize) < 0;
+ }
+
+ /**
+ * Attempt to address a device on the I2C bus.
+ *
+ * <p>This allows you to figure out if there is a device on the I2C bus that responds to the
+ * address specified in the constructor.
+ *
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public boolean addressOnly() {
+ return transaction(new byte[0], (byte) 0, new byte[0], (byte) 0);
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write a single byte to a register on a device and wait until the transaction is complete.
+ *
+ * @param registerAddress The address of the register on the device to be written.
+ * @param data The byte to write to the register on the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean write(int registerAddress, int data) {
+ byte[] buffer = new byte[2];
+ buffer[0] = (byte) registerAddress;
+ buffer[1] = (byte) data;
+ return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, buffer,
+ (byte) buffer.length) < 0;
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+ *
+ * @param data The data to write to the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean writeBulk(byte[] data) {
+ return writeBulk(data, data.length);
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+ *
+ * @param data The data to write to the device.
+ * @param size The number of data bytes to write.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean writeBulk(byte[] data, int size) {
+ if (data.length < size) {
+ throw new IllegalArgumentException(
+ "buffer is too small, must be at least " + size);
+ }
+ return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+ *
+ * @param data The data to write to the device.
+ * @param size The number of data bytes to write.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public synchronized boolean writeBulk(ByteBuffer data, int size) {
+ if (data.hasArray()) {
+ return writeBulk(data.array(), size);
+ }
+ if (!data.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (data.capacity() < size) {
+ throw new IllegalArgumentException(
+ "buffer is too small, must be at least " + size);
+ }
+
+ return I2CJNI.i2CWrite(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
+ }
+
+ /**
+ * Execute a read transaction with the device.
+ *
+ * <p>Read bytes from a device. Most I2C devices will auto-increment the register pointer
+ * internally allowing you to read consecutive registers on a device in a single transaction.
+ *
+ * @param registerAddress The register to read first in the transaction.
+ * @param count The number of bytes to read in the transaction.
+ * @param buffer A pointer to the array of bytes to store the data read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public boolean read(int registerAddress, int count, byte[] buffer) {
+ requireNonNullParam(buffer, "buffer", "read");
+
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count + " given");
+ }
+ if (buffer.length < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ byte[] registerAddressArray = new byte[1];
+ registerAddressArray[0] = (byte) registerAddress;
+
+ return transaction(registerAddressArray, registerAddressArray.length, buffer, count);
+ }
+
+ private ByteBuffer m_readDataToSendBuffer;
+
+ /**
+ * Execute a read transaction with the device.
+ *
+ * <p>Read bytes from a device. Most I2C devices will auto-increment the register pointer
+ * internally allowing you to read consecutive registers on a device in a single transaction.
+ *
+ * @param registerAddress The register to read first in the transaction.
+ * @param count The number of bytes to read in the transaction.
+ * @param buffer A buffer to store the data read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public boolean read(int registerAddress, int count, ByteBuffer buffer) {
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count + " given");
+ }
+
+ if (buffer.hasArray()) {
+ return read(registerAddress, count, buffer.array());
+ }
+
+ if (!buffer.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (buffer.capacity() < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ synchronized (this) {
+ if (m_readDataToSendBuffer == null) {
+ m_readDataToSendBuffer = ByteBuffer.allocateDirect(1);
+ }
+ m_readDataToSendBuffer.put(0, (byte) registerAddress);
+
+ return transaction(m_readDataToSendBuffer, 1, buffer, count);
+ }
+ }
+
+ /**
+ * Execute a read only transaction with the device.
+ *
+ * <p>Read bytes from a device. This method does not write any data to prompt the device.
+ *
+ * @param buffer A pointer to the array of bytes to store the data read from the device.
+ * @param count The number of bytes to read in the transaction.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public boolean readOnly(byte[] buffer, int count) {
+ requireNonNullParam(buffer, "buffer", "readOnly");
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count + " given");
+ }
+ if (buffer.length < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ return I2CJNI.i2CReadB(m_port, (byte) m_deviceAddress, buffer,
+ (byte) count) < 0;
+ }
+
+ /**
+ * Execute a read only transaction with the device.
+ *
+ * <p>Read bytes from a device. This method does not write any data to prompt the device.
+ *
+ * @param buffer A pointer to the array of bytes to store the data read from the device.
+ * @param count The number of bytes to read in the transaction.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public boolean readOnly(ByteBuffer buffer, int count) {
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count
+ + " given");
+ }
+
+ if (buffer.hasArray()) {
+ return readOnly(buffer.array(), count);
+ }
+
+ if (!buffer.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (buffer.capacity() < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ return I2CJNI.i2CRead(m_port, (byte) m_deviceAddress, buffer, (byte) count)
+ < 0;
+ }
+
+ /*
+ * Send a broadcast write to all devices on the I2C bus.
+ *
+ * <p>This is not currently implemented!
+ *
+ * @param registerAddress The register to write on all devices on the bus.
+ * @param data The value to write to the devices.
+ */
+ // public void broadcast(int registerAddress, int data) {
+ // }
+
+ /**
+ * Verify that a device's registers contain expected values.
+ *
+ * <p>Most devices will have a set of registers that contain a known value that can be used to
+ * identify them. This allows an I2C device driver to easily verify that the device contains the
+ * expected value.
+ *
+ * @param registerAddress The base register to start reading from the device.
+ * @param count The size of the field to be verified.
+ * @param expected A buffer containing the values expected from the device.
+ * @return true if the sensor was verified to be connected
+ * @pre The device must support and be configured to use register auto-increment.
+ */
+ public boolean verifySensor(int registerAddress, int count,
+ byte[] expected) {
+ // TODO: Make use of all 7 read bytes
+ byte[] dataToSend = new byte[1];
+
+ byte[] deviceData = new byte[4];
+ for (int i = 0; i < count; i += 4) {
+ int toRead = count - i < 4 ? count - i : 4;
+ // Read the chunk of data. Return false if the sensor does not
+ // respond.
+ dataToSend[0] = (byte) (registerAddress + i);
+ if (transaction(dataToSend, 1, deviceData, toRead)) {
+ return false;
+ }
+
+ for (byte j = 0; j < toRead; j++) {
+ if (deviceData[j] != expected[i + j]) {
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java
new file mode 100644
index 0000000..6fd807b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.InterruptJNI.InterruptJNIHandlerFunction;
+
+
+/**
+ * It is recommended that you use this class in conjunction with classes from {@link
+ * java.util.concurrent.atomic} as these objects are all thread safe.
+ *
+ * @param <T> The type of the parameter that should be returned to the the method {@link
+ * #interruptFired(int, Object)}
+ */
+public abstract class InterruptHandlerFunction<T> {
+ /**
+ * The entry point for the interrupt. When the interrupt fires the {@link #apply(int, Object)}
+ * method is called. The outer class is provided as an interface to allow the implementer to pass
+ * a generic object to the interrupt fired method.
+ */
+ private class Function implements InterruptJNIHandlerFunction {
+ @SuppressWarnings("unchecked")
+ @Override
+ public void apply(int interruptAssertedMask, Object param) {
+ interruptFired(interruptAssertedMask, (T) param);
+ }
+ }
+
+ final Function m_function = new Function();
+
+ /**
+ * This method is run every time an interrupt is fired.
+ *
+ * @param interruptAssertedMask Interrupt Mask
+ * @param param The parameter provided by overriding the {@link
+ * #overridableParameter()} method.
+ */
+ public abstract void interruptFired(int interruptAssertedMask, T param);
+
+
+ /**
+ * Override this method if you would like to pass a specific parameter to the {@link
+ * #interruptFired(int, Object)} when it is fired by the interrupt. This method is called once
+ * when {@link InterruptableSensorBase#requestInterrupts(InterruptHandlerFunction)} is run.
+ *
+ * @return The object that should be passed to the interrupt when it runs
+ */
+ public T overridableParameter() {
+ return null;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
new file mode 100644
index 0000000..bd21d24
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
@@ -0,0 +1,284 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.function.Consumer;
+
+import edu.wpi.first.hal.InterruptJNI;
+import edu.wpi.first.hal.util.AllocationException;
+
+
+/**
+ * Base for sensors to be used with interrupts.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class InterruptableSensorBase implements AutoCloseable {
+ @SuppressWarnings("JavadocMethod")
+ public enum WaitResult {
+ kTimeout(0x0), kRisingEdge(0x1), kFallingEdge(0x100), kBoth(0x101);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ WaitResult(int value) {
+ this.value = value;
+ }
+
+ public static WaitResult getValue(boolean rising, boolean falling) {
+ if (rising && falling) {
+ return kBoth;
+ } else if (rising) {
+ return kRisingEdge;
+ } else if (falling) {
+ return kFallingEdge;
+ } else {
+ return kTimeout;
+ }
+ }
+ }
+
+ /**
+ * The interrupt resource.
+ */
+ protected int m_interrupt = InterruptJNI.HalInvalidHandle;
+
+ /**
+ * Flags if the interrupt being allocated is synchronous.
+ */
+ protected boolean m_isSynchronousInterrupt;
+
+ /**
+ * Create a new InterrupatableSensorBase.
+ */
+ public InterruptableSensorBase() {
+ m_interrupt = 0;
+ }
+
+ @Override
+ public void close() {
+ if (m_interrupt != 0) {
+ cancelInterrupts();
+ }
+ }
+
+ /**
+ * If this is an analog trigger.
+ *
+ * @return true if this is an analog trigger.
+ */
+ public abstract int getAnalogTriggerTypeForRouting();
+
+ /**
+ * The channel routing number.
+ *
+ * @return channel routing number
+ */
+ public abstract int getPortHandleForRouting();
+
+ /**
+ * Request one of the 8 interrupts asynchronously on this digital input.
+ *
+ * @param handler The {@link Consumer} that will be called whenever there is an interrupt on this
+ * device. Request interrupts in synchronous mode where the user program interrupt
+ * handler will be called when an interrupt occurs. The default is interrupt on
+ * rising edges only.
+ */
+ public void requestInterrupts(Consumer<WaitResult> handler) {
+ if (m_interrupt != 0) {
+ throw new AllocationException("The interrupt has already been allocated");
+ }
+
+ allocateInterrupts(false);
+
+ assert m_interrupt != 0;
+
+ InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
+ getAnalogTriggerTypeForRouting());
+ setUpSourceEdge(true, false);
+ InterruptJNI.attachInterruptHandler(m_interrupt, (mask, obj) -> {
+ // Rising edge result is the interrupt bit set in the byte 0xFF
+ // Falling edge result is the interrupt bit set in the byte 0xFF00
+ boolean rising = (mask & 0xFF) != 0;
+ boolean falling = (mask & 0xFF00) != 0;
+ handler.accept(WaitResult.getValue(rising, falling));
+ }, null);
+ }
+
+ /**
+ * Request one of the 8 interrupts asynchronously on this digital input.
+ *
+ * @param handler The {@link InterruptHandlerFunction} that contains the method {@link
+ * InterruptHandlerFunction#interruptFired(int, Object)} that will be called
+ * whenever there is an interrupt on this device. Request interrupts in synchronous
+ * mode where the user program interrupt handler will be called when an interrupt
+ * occurs. The default is interrupt on rising edges only.
+ */
+ public void requestInterrupts(InterruptHandlerFunction<?> handler) {
+ if (m_interrupt != 0) {
+ throw new AllocationException("The interrupt has already been allocated");
+ }
+
+ allocateInterrupts(false);
+
+ assert m_interrupt != 0;
+
+ InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
+ getAnalogTriggerTypeForRouting());
+ setUpSourceEdge(true, false);
+ InterruptJNI.attachInterruptHandler(m_interrupt, handler.m_function,
+ handler.overridableParameter());
+ }
+
+ /**
+ * Request one of the 8 interrupts synchronously on this digital input. Request interrupts in
+ * synchronous mode where the user program will have to explicitly wait for the interrupt to occur
+ * using {@link #waitForInterrupt}. The default is interrupt on rising edges only.
+ */
+ public void requestInterrupts() {
+ if (m_interrupt != 0) {
+ throw new AllocationException("The interrupt has already been allocated");
+ }
+
+ allocateInterrupts(true);
+
+ assert m_interrupt != 0;
+
+ InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
+ getAnalogTriggerTypeForRouting());
+ setUpSourceEdge(true, false);
+
+ }
+
+ /**
+ * Allocate the interrupt.
+ *
+ * @param watcher true if the interrupt should be in synchronous mode where the user program will
+ * have to explicitly wait for the interrupt to occur.
+ */
+ protected void allocateInterrupts(boolean watcher) {
+ m_isSynchronousInterrupt = watcher;
+
+ m_interrupt = InterruptJNI.initializeInterrupts(watcher);
+ }
+
+ /**
+ * Cancel interrupts on this device. This deallocates all the chipobject structures and disables
+ * any interrupts.
+ */
+ public void cancelInterrupts() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ InterruptJNI.cleanInterrupts(m_interrupt);
+ m_interrupt = 0;
+ }
+
+ /**
+ * In synchronous mode, wait for the defined interrupt to occur.
+ *
+ * @param timeout Timeout in seconds
+ * @param ignorePrevious If true, ignore interrupts that happened before waitForInterrupt was
+ * called.
+ * @return Result of the wait.
+ */
+ public WaitResult waitForInterrupt(double timeout, boolean ignorePrevious) {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ int result = InterruptJNI.waitForInterrupt(m_interrupt, timeout, ignorePrevious);
+
+ // Rising edge result is the interrupt bit set in the byte 0xFF
+ // Falling edge result is the interrupt bit set in the byte 0xFF00
+ // Set any bit set to be true for that edge, and AND the 2 results
+ // together to match the existing enum for all interrupts
+ boolean rising = (result & 0xFF) != 0;
+ boolean falling = (result & 0xFF00) != 0;
+ return WaitResult.getValue(rising, falling);
+ }
+
+ /**
+ * In synchronous mode, wait for the defined interrupt to occur.
+ *
+ * @param timeout Timeout in seconds
+ * @return Result of the wait.
+ */
+ public WaitResult waitForInterrupt(double timeout) {
+ return waitForInterrupt(timeout, true);
+ }
+
+ /**
+ * Enable interrupts to occur on this input. Interrupts are disabled when the RequestInterrupt
+ * call is made. This gives time to do the setup of the other options before starting to field
+ * interrupts.
+ */
+ public void enableInterrupts() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ if (m_isSynchronousInterrupt) {
+ throw new IllegalStateException("You do not need to enable synchronous interrupts");
+ }
+ InterruptJNI.enableInterrupts(m_interrupt);
+ }
+
+ /**
+ * Disable Interrupts without without deallocating structures.
+ */
+ public void disableInterrupts() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ if (m_isSynchronousInterrupt) {
+ throw new IllegalStateException("You can not disable synchronous interrupts");
+ }
+ InterruptJNI.disableInterrupts(m_interrupt);
+ }
+
+ /**
+ * Return the timestamp for the rising interrupt that occurred most recently. This is in the same
+ * time domain as getClock(). The rising-edge interrupt should be enabled with {@link
+ * #setUpSourceEdge}.
+ *
+ * @return Timestamp in seconds since boot.
+ */
+ public double readRisingTimestamp() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ return InterruptJNI.readInterruptRisingTimestamp(m_interrupt) * 1e-6;
+ }
+
+ /**
+ * Return the timestamp for the falling interrupt that occurred most recently. This is in the same
+ * time domain as getClock(). The falling-edge interrupt should be enabled with {@link
+ * #setUpSourceEdge}.
+ *
+ * @return Timestamp in seconds since boot.
+ */
+ public double readFallingTimestamp() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ return InterruptJNI.readInterruptFallingTimestamp(m_interrupt) * 1e-6;
+ }
+
+ /**
+ * Set which edge to trigger interrupts on.
+ *
+ * @param risingEdge true to interrupt on rising edge
+ * @param fallingEdge true to interrupt on falling edge
+ */
+ public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
+ if (m_interrupt != 0) {
+ InterruptJNI.setInterruptUpSourceEdge(m_interrupt, risingEdge,
+ fallingEdge);
+ } else {
+ throw new IllegalArgumentException("You must call RequestInterrupts before setUpSourceEdge");
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
new file mode 100644
index 0000000..f1baefb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * IterativeRobot implements the IterativeRobotBase robot program framework.
+ *
+ * <p>The IterativeRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>periodic() functions from the base class are called each time a new packet is received from
+ * the driver station.
+ *
+ * @deprecated Use TimedRobot instead. It's a drop-in replacement that provides more regular
+ * execution periods.
+ */
+@Deprecated
+public class IterativeRobot extends IterativeRobotBase {
+ private static final double kPacketPeriod = 0.02;
+ private volatile boolean m_exit;
+
+ /**
+ * Create a new IterativeRobot.
+ */
+ public IterativeRobot() {
+ super(kPacketPeriod);
+
+ HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ @Override
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ // Loop forever, calling the appropriate mode-dependent function
+ while (!Thread.currentThread().isInterrupted()) {
+ // Wait for new data to arrive
+ m_ds.waitForData();
+ if (m_exit) {
+ break;
+ }
+
+ loopFunc();
+ }
+ }
+
+ /**
+ * Ends the main loop in startCompetition().
+ */
+ @Override
+ public void endCompetition() {
+ m_exit = true;
+ m_ds.wakeupWaitForData();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
new file mode 100644
index 0000000..b9a7a6a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
@@ -0,0 +1,278 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
+ * class.
+ *
+ * <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
+ * by teams directly.
+ *
+ * <p>This class provides the following functions which are called by the main loop,
+ * startCompetition(), at the appropriate times:
+ *
+ * <p>robotInit() -- provide for initialization at robot power-on
+ *
+ * <p>init() functions -- each of the following functions is called once when the
+ * appropriate mode is entered:
+ * - disabledInit() -- called each and every time disabled is entered from
+ * another mode
+ * - autonomousInit() -- called each and every time autonomous is entered from
+ * another mode
+ * - teleopInit() -- called each and every time teleop is entered from
+ * another mode
+ * - testInit() -- called each and every time test is entered from
+ * another mode
+ *
+ * <p>periodic() functions -- each of these functions is called on an interval:
+ * - robotPeriodic()
+ * - disabledPeriodic()
+ * - autonomousPeriodic()
+ * - teleopPeriodic()
+ * - testPeriodic()
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class IterativeRobotBase extends RobotBase {
+ protected double m_period;
+
+ private enum Mode {
+ kNone,
+ kDisabled,
+ kAutonomous,
+ kTeleop,
+ kTest
+ }
+
+ private Mode m_lastMode = Mode.kNone;
+ private final Watchdog m_watchdog;
+
+ /**
+ * Constructor for IterativeRobotBase.
+ *
+ * @param period Period in seconds.
+ */
+ protected IterativeRobotBase(double period) {
+ m_period = period;
+ m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ @Override
+ public abstract void startCompetition();
+
+ /* ----------- Overridable initialization code ----------------- */
+
+ /**
+ * Robot-wide initialization code should go here.
+ *
+ * <p>Users should override this method for default Robot-wide initialization which will be called
+ * when the robot is first powered on. It will be called exactly one time.
+ *
+ * <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
+ * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
+ * never indicate that the code is ready, causing the robot to be bypassed in a match.
+ */
+ public void robotInit() {
+ System.out.println("Default robotInit() method... Override me!");
+ }
+
+ /**
+ * Initialization code for disabled mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters disabled mode.
+ */
+ public void disabledInit() {
+ System.out.println("Default disabledInit() method... Override me!");
+ }
+
+ /**
+ * Initialization code for autonomous mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters autonomous mode.
+ */
+ public void autonomousInit() {
+ System.out.println("Default autonomousInit() method... Override me!");
+ }
+
+ /**
+ * Initialization code for teleop mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters teleop mode.
+ */
+ public void teleopInit() {
+ System.out.println("Default teleopInit() method... Override me!");
+ }
+
+ /**
+ * Initialization code for test mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters test mode.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void testInit() {
+ System.out.println("Default testInit() method... Override me!");
+ }
+
+ /* ----------- Overridable periodic code ----------------- */
+
+ private boolean m_rpFirstRun = true;
+
+ /**
+ * Periodic code for all robot modes should go here.
+ */
+ public void robotPeriodic() {
+ if (m_rpFirstRun) {
+ System.out.println("Default robotPeriodic() method... Override me!");
+ m_rpFirstRun = false;
+ }
+ }
+
+ private boolean m_dpFirstRun = true;
+
+ /**
+ * Periodic code for disabled mode should go here.
+ */
+ public void disabledPeriodic() {
+ if (m_dpFirstRun) {
+ System.out.println("Default disabledPeriodic() method... Override me!");
+ m_dpFirstRun = false;
+ }
+ }
+
+ private boolean m_apFirstRun = true;
+
+ /**
+ * Periodic code for autonomous mode should go here.
+ */
+ public void autonomousPeriodic() {
+ if (m_apFirstRun) {
+ System.out.println("Default autonomousPeriodic() method... Override me!");
+ m_apFirstRun = false;
+ }
+ }
+
+ private boolean m_tpFirstRun = true;
+
+ /**
+ * Periodic code for teleop mode should go here.
+ */
+ public void teleopPeriodic() {
+ if (m_tpFirstRun) {
+ System.out.println("Default teleopPeriodic() method... Override me!");
+ m_tpFirstRun = false;
+ }
+ }
+
+ private boolean m_tmpFirstRun = true;
+
+ /**
+ * Periodic code for test mode should go here.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void testPeriodic() {
+ if (m_tmpFirstRun) {
+ System.out.println("Default testPeriodic() method... Override me!");
+ m_tmpFirstRun = false;
+ }
+ }
+
+ protected void loopFunc() {
+ m_watchdog.reset();
+
+ // Call the appropriate function depending upon the current robot mode
+ if (isDisabled()) {
+ // Call DisabledInit() if we are now just entering disabled mode from either a different mode
+ // or from power-on.
+ if (m_lastMode != Mode.kDisabled) {
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ disabledInit();
+ m_watchdog.addEpoch("disabledInit()");
+ m_lastMode = Mode.kDisabled;
+ }
+
+ HAL.observeUserProgramDisabled();
+ disabledPeriodic();
+ m_watchdog.addEpoch("disablePeriodic()");
+ } else if (isAutonomous()) {
+ // Call AutonomousInit() if we are now just entering autonomous mode from either a different
+ // mode or from power-on.
+ if (m_lastMode != Mode.kAutonomous) {
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ autonomousInit();
+ m_watchdog.addEpoch("autonomousInit()");
+ m_lastMode = Mode.kAutonomous;
+ }
+
+ HAL.observeUserProgramAutonomous();
+ autonomousPeriodic();
+ m_watchdog.addEpoch("autonomousPeriodic()");
+ } else if (isOperatorControl()) {
+ // Call TeleopInit() if we are now just entering teleop mode from either a different mode or
+ // from power-on.
+ if (m_lastMode != Mode.kTeleop) {
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ teleopInit();
+ m_watchdog.addEpoch("teleopInit()");
+ m_lastMode = Mode.kTeleop;
+ }
+
+ HAL.observeUserProgramTeleop();
+ teleopPeriodic();
+ m_watchdog.addEpoch("teleopPeriodic()");
+ } else {
+ // Call TestInit() if we are now just entering test mode from either a different mode or from
+ // power-on.
+ if (m_lastMode != Mode.kTest) {
+ LiveWindow.setEnabled(true);
+ Shuffleboard.enableActuatorWidgets();
+ testInit();
+ m_watchdog.addEpoch("testInit()");
+ m_lastMode = Mode.kTest;
+ }
+
+ HAL.observeUserProgramTest();
+ testPeriodic();
+ m_watchdog.addEpoch("testPeriodic()");
+ }
+
+ robotPeriodic();
+ m_watchdog.addEpoch("robotPeriodic()");
+
+ SmartDashboard.updateValues();
+ m_watchdog.addEpoch("SmartDashboard.updateValues()");
+ LiveWindow.updateValues();
+ m_watchdog.addEpoch("LiveWindow.updateValues()");
+ Shuffleboard.update();
+ m_watchdog.addEpoch("Shuffleboard.update()");
+ m_watchdog.disable();
+
+ // Warn on loop time overruns
+ if (m_watchdog.isExpired()) {
+ m_watchdog.printEpochs();
+ }
+ }
+
+ private void printLoopOverrunMessage() {
+ DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
new file mode 100644
index 0000000..8ce7b7d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device.
+ *
+ * <p>Note that the Jaguar uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the Jaguar User Manual
+ * available from Vex.
+ *
+ * <p><ul>
+ * <li>2.310ms = full "forward"
+ * <li>1.550ms = the "high end" of the deadband range
+ * <li>1.507ms = center of the deadband range (off)
+ * <li>1.454ms = the "low end" of the deadband range
+ * <li>0.697ms = full "reverse"
+ * </ul>
+ */
+public class Jaguar extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the Jaguar is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Jaguar(final int channel) {
+ super(channel);
+
+ setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_Jaguar, getChannel() + 1);
+ SendableRegistry.setName(this, "Jaguar", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
new file mode 100644
index 0000000..60a60bb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -0,0 +1,327 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Handle input from standard Joysticks connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each joystick
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class Joystick extends GenericHID {
+ static final byte kDefaultXChannel = 0;
+ static final byte kDefaultYChannel = 1;
+ static final byte kDefaultZChannel = 2;
+ static final byte kDefaultTwistChannel = 2;
+ static final byte kDefaultThrottleChannel = 3;
+
+ /**
+ * Represents an analog axis on a joystick.
+ */
+ public enum AxisType {
+ kX(0), kY(1), kZ(2), kTwist(3), kThrottle(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ AxisType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents a digital button on a joystick.
+ */
+ public enum ButtonType {
+ kTrigger(1), kTop(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ ButtonType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents a digital button on a joystick.
+ */
+ private enum Button {
+ kTrigger(1), kTop(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Button(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents an analog axis on a joystick.
+ */
+ private enum Axis {
+ kX(0), kY(1), kZ(2), kTwist(3), kThrottle(4), kNumAxes(5);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Axis(int value) {
+ this.value = value;
+ }
+ }
+
+ private final byte[] m_axes = new byte[Axis.kNumAxes.value];
+
+ /**
+ * Construct an instance of a joystick. The joystick index is the USB port on the drivers
+ * station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into.
+ */
+ public Joystick(final int port) {
+ super(port);
+
+ m_axes[Axis.kX.value] = kDefaultXChannel;
+ m_axes[Axis.kY.value] = kDefaultYChannel;
+ m_axes[Axis.kZ.value] = kDefaultZChannel;
+ m_axes[Axis.kTwist.value] = kDefaultTwistChannel;
+ m_axes[Axis.kThrottle.value] = kDefaultThrottleChannel;
+
+ HAL.report(tResourceType.kResourceType_Joystick, port + 1);
+ }
+
+ /**
+ * Set the channel associated with the X axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setXChannel(int channel) {
+ m_axes[Axis.kX.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the Y axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setYChannel(int channel) {
+ m_axes[Axis.kY.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the Z axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setZChannel(int channel) {
+ m_axes[Axis.kZ.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the throttle axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setThrottleChannel(int channel) {
+ m_axes[Axis.kThrottle.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the twist axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setTwistChannel(int channel) {
+ m_axes[Axis.kTwist.value] = (byte) channel;
+ }
+
+ /**
+ * Get the channel currently associated with the X axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getXChannel() {
+ return m_axes[Axis.kX.value];
+ }
+
+ /**
+ * Get the channel currently associated with the Y axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getYChannel() {
+ return m_axes[Axis.kY.value];
+ }
+
+ /**
+ * Get the channel currently associated with the Z axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getZChannel() {
+ return m_axes[Axis.kZ.value];
+ }
+
+ /**
+ * Get the channel currently associated with the twist axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getTwistChannel() {
+ return m_axes[Axis.kTwist.value];
+ }
+
+ /**
+ * Get the channel currently associated with the throttle axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getThrottleChannel() {
+ return m_axes[Axis.kThrottle.value];
+ }
+
+ /**
+ * Get the X value of the joystick. This depends on the mapping of the joystick connected to the
+ * current port.
+ *
+ * @param hand Unused
+ * @return The X value of the joystick.
+ */
+ @Override
+ public final double getX(Hand hand) {
+ return getRawAxis(m_axes[Axis.kX.value]);
+ }
+
+ /**
+ * Get the Y value of the joystick. This depends on the mapping of the joystick connected to the
+ * current port.
+ *
+ * @param hand Unused
+ * @return The Y value of the joystick.
+ */
+ @Override
+ public final double getY(Hand hand) {
+ return getRawAxis(m_axes[Axis.kY.value]);
+ }
+
+ /**
+ * Get the z position of the HID.
+ *
+ * @return the z position
+ */
+ public double getZ() {
+ return getRawAxis(m_axes[Axis.kZ.value]);
+ }
+
+ /**
+ * Get the twist value of the current joystick. This depends on the mapping of the joystick
+ * connected to the current port.
+ *
+ * @return The Twist value of the joystick.
+ */
+ public double getTwist() {
+ return getRawAxis(m_axes[Axis.kTwist.value]);
+ }
+
+ /**
+ * Get the throttle value of the current joystick. This depends on the mapping of the joystick
+ * connected to the current port.
+ *
+ * @return The Throttle value of the joystick.
+ */
+ public double getThrottle() {
+ return getRawAxis(m_axes[Axis.kThrottle.value]);
+ }
+
+ /**
+ * Read the state of the trigger on the joystick.
+ *
+ * @return The state of the trigger.
+ */
+ public boolean getTrigger() {
+ return getRawButton(Button.kTrigger.value);
+ }
+
+ /**
+ * Whether the trigger was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getTriggerPressed() {
+ return getRawButtonPressed(Button.kTrigger.value);
+ }
+
+ /**
+ * Whether the trigger was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getTriggerReleased() {
+ return getRawButtonReleased(Button.kTrigger.value);
+ }
+
+ /**
+ * Read the state of the top button on the joystick.
+ *
+ * @return The state of the top button.
+ */
+ public boolean getTop() {
+ return getRawButton(Button.kTop.value);
+ }
+
+ /**
+ * Whether the top button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getTopPressed() {
+ return getRawButtonPressed(Button.kTop.value);
+ }
+
+ /**
+ * Whether the top button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getTopReleased() {
+ return getRawButtonReleased(Button.kTop.value);
+ }
+
+ /**
+ * Get the magnitude of the direction vector formed by the joystick's current position relative to
+ * its origin.
+ *
+ * @return The magnitude of the direction vector
+ */
+ public double getMagnitude() {
+ return Math.sqrt(Math.pow(getX(), 2) + Math.pow(getY(), 2));
+ }
+
+ /**
+ * Get the direction of the vector formed by the joystick and its origin in radians.
+ *
+ * @return The direction of the vector in radians
+ */
+ public double getDirectionRadians() {
+ return Math.atan2(getX(), -getY());
+ }
+
+ /**
+ * Get the direction of the vector formed by the joystick and its origin in degrees.
+ *
+ * @return The direction of the vector in degrees
+ */
+ public double getDirectionDegrees() {
+ return Math.toDegrees(getDirectionRadians());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
new file mode 100644
index 0000000..94bcb31
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Arrays;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
+ * Static factory methods are provided to create commonly used types of filters.
+ *
+ * <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
+ * a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ * <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
+ * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
+ * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
+ * impact of these high frequency components. Likewise, a "high pass" filter gets rid of
+ * slow-moving signal components, letting you detect large changes more easily.
+ *
+ * <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
+ * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
+ * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
+ * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
+ * PID controller out of this class!
+ *
+ * <p>For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
+ * Notifier for this or do it "inline" with code in a periodic function.
+ *
+ * <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
+ * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
+ * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
+ * to make sure calculate() gets called at the desired, constant frequency!
+ */
+public class LinearFilter {
+ private final CircularBuffer m_inputs;
+ private final CircularBuffer m_outputs;
+ private final double[] m_inputGains;
+ private final double[] m_outputGains;
+
+ private static int instances;
+
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param ffGains The "feed forward" or FIR gains.
+ * @param fbGains The "feed back" or IIR gains.
+ */
+ public LinearFilter(double[] ffGains, double[] fbGains) {
+ m_inputs = new CircularBuffer(ffGains.length);
+ m_outputs = new CircularBuffer(fbGains.length);
+ m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+ m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+ instances++;
+ HAL.report(tResourceType.kResourceType_LinearFilter, instances);
+ }
+
+ /**
+ * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
+ * gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ * <p>This filter is stable for time constants greater than zero.
+ *
+ * @param timeConstant The discrete-time time constant in seconds.
+ * @param period The period in seconds between samples taken by the user.
+ */
+ public static LinearFilter singlePoleIIR(double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {1.0 - gain};
+ double[] fbGains = {-gain};
+
+ return new LinearFilter(ffGains, fbGains);
+ }
+
+ /**
+ * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
+ * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ * <p>This filter is stable for time constants greater than zero.
+ *
+ * @param timeConstant The discrete-time time constant in seconds.
+ * @param period The period in seconds between samples taken by the user.
+ */
+ public static LinearFilter highPass(double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {gain, -gain};
+ double[] fbGains = {-gain};
+
+ return new LinearFilter(ffGains, fbGains);
+ }
+
+ /**
+ * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
+ * x[0]).
+ *
+ * <p>This filter is always stable.
+ *
+ * @param taps The number of samples to average over. Higher = smoother but slower.
+ * @throws IllegalArgumentException if number of taps is less than 1.
+ */
+ public static LinearFilter movingAverage(int taps) {
+ if (taps <= 0) {
+ throw new IllegalArgumentException("Number of taps was not at least 1");
+ }
+
+ double[] ffGains = new double[taps];
+ for (int i = 0; i < ffGains.length; i++) {
+ ffGains[i] = 1.0 / taps;
+ }
+
+ double[] fbGains = new double[0];
+
+ return new LinearFilter(ffGains, fbGains);
+ }
+
+ /**
+ * Reset the filter state.
+ */
+ public void reset() {
+ m_inputs.clear();
+ m_outputs.clear();
+ }
+
+ /**
+ * Calculates the next value of the filter.
+ *
+ * @param input Current input value.
+ *
+ * @return The filtered value at this step
+ */
+ public double calculate(double input) {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.addFirst(input);
+
+ // Calculate the new value
+ for (int i = 0; i < m_inputGains.length; i++) {
+ retVal += m_inputs.get(i) * m_inputGains[i];
+ }
+ for (int i = 0; i < m_outputGains.length; i++) {
+ retVal -= m_outputs.get(i) * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.addFirst(retVal);
+
+ return retVal;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
new file mode 100644
index 0000000..a4a3a95
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * A class that implements a moving-window median filter. Useful for reducing measurement noise,
+ * especially with processes that generate occasional, extreme outliers (such as values from
+ * vision processing, LIDAR, or ultrasonic sensors).
+ */
+public class MedianFilter {
+ private final CircularBuffer m_valueBuffer;
+ private final List<Double> m_orderedValues;
+ private final int m_size;
+
+ /**
+ * Creates a new MedianFilter.
+ *
+ * @param size The number of samples in the moving window.
+ */
+ public MedianFilter(int size) {
+ // Circular buffer of values currently in the window, ordered by time
+ m_valueBuffer = new CircularBuffer(size);
+ // List of values currently in the window, ordered by value
+ m_orderedValues = new ArrayList<>(size);
+ // Size of rolling window
+ m_size = size;
+ }
+
+ /**
+ * Calculates the moving-window median for the next value of the input stream.
+ *
+ * @param next The next input value.
+ * @return The median of the moving window, updated to include the next value.
+ */
+ public double calculate(double next) {
+ // Find insertion point for next value
+ int index = Collections.binarySearch(m_orderedValues, next);
+
+ // Deal with binarySearch behavior for element not found
+ if (index < 0) {
+ index = Math.abs(index + 1);
+ }
+
+ // Place value at proper insertion point
+ m_orderedValues.add(index, next);
+
+ int curSize = m_orderedValues.size();
+
+ // If buffer is at max size, pop element off of end of circular buffer
+ // and remove from ordered list
+ if (curSize > m_size) {
+ m_orderedValues.remove(m_valueBuffer.removeLast());
+ curSize = curSize - 1;
+ }
+
+ // Add next value to circular buffer
+ m_valueBuffer.addFirst(next);
+
+ if (curSize % 2 == 1) {
+ // If size is odd, return middle element of sorted list
+ return m_orderedValues.get(curSize / 2);
+ } else {
+ // If size is even, return average of middle elements
+ return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+ }
+ }
+
+ /**
+ * Resets the filter, clearing the window of all elements.
+ */
+ public void reset() {
+ m_orderedValues.clear();
+ m_valueBuffer.clear();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
new file mode 100644
index 0000000..5dd3a4f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.LinkedHashSet;
+import java.util.Set;
+
+/**
+ * This base class runs a watchdog timer and calls the subclass's StopMotor()
+ * function if the timeout expires.
+ *
+ * <p>The subclass should call feed() whenever the motor value is updated.
+ */
+public abstract class MotorSafety {
+ private static final double kDefaultSafetyExpiration = 0.1;
+
+ private double m_expiration = kDefaultSafetyExpiration;
+ private boolean m_enabled;
+ private double m_stopTime = Timer.getFPGATimestamp();
+ private final Object m_thisMutex = new Object();
+ private static final Set<MotorSafety> m_instanceList = new LinkedHashSet<>();
+ private static final Object m_listMutex = new Object();
+
+ /**
+ * MotorSafety constructor.
+ */
+ public MotorSafety() {
+ synchronized (m_listMutex) {
+ m_instanceList.add(this);
+ }
+ }
+
+ /**
+ * Feed the motor safety object.
+ *
+ * <p>Resets the timer on this object that is used to do the timeouts.
+ */
+ public void feed() {
+ synchronized (m_thisMutex) {
+ m_stopTime = Timer.getFPGATimestamp() + m_expiration;
+ }
+ }
+
+ /**
+ * Set the expiration time for the corresponding motor safety object.
+ *
+ * @param expirationTime The timeout value in seconds.
+ */
+ public void setExpiration(double expirationTime) {
+ synchronized (m_thisMutex) {
+ m_expiration = expirationTime;
+ }
+ }
+
+ /**
+ * Retrieve the timeout value for the corresponding motor safety object.
+ *
+ * @return the timeout value in seconds.
+ */
+ public double getExpiration() {
+ synchronized (m_thisMutex) {
+ return m_expiration;
+ }
+ }
+
+ /**
+ * Determine of the motor is still operating or has timed out.
+ *
+ * @return a true value if the motor is still operating normally and hasn't timed out.
+ */
+ public boolean isAlive() {
+ synchronized (m_thisMutex) {
+ return !m_enabled || m_stopTime > Timer.getFPGATimestamp();
+ }
+ }
+
+ /**
+ * Check if this motor has exceeded its timeout. This method is called periodically to determine
+ * if this motor has exceeded its timeout value. If it has, the stop method is called, and the
+ * motor is shut down until its value is updated again.
+ */
+ public void check() {
+ boolean enabled;
+ double stopTime;
+
+ synchronized (m_thisMutex) {
+ enabled = m_enabled;
+ stopTime = m_stopTime;
+ }
+
+ if (!enabled || RobotState.isDisabled() || RobotState.isTest()) {
+ return;
+ }
+
+ if (stopTime < Timer.getFPGATimestamp()) {
+ DriverStation.reportError(getDescription() + "... Output not updated often enough.", false);
+
+ stopMotor();
+ }
+ }
+
+ /**
+ * Enable/disable motor safety for this device.
+ *
+ * <p>Turn on and off the motor safety option for this PWM object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+ public void setSafetyEnabled(boolean enabled) {
+ synchronized (m_thisMutex) {
+ m_enabled = enabled;
+ }
+ }
+
+ /**
+ * Return the state of the motor safety enabled flag.
+ *
+ * <p>Return if the motor safety is currently enabled for this device.
+ *
+ * @return True if motor safety is enforced for this device
+ */
+ public boolean isSafetyEnabled() {
+ synchronized (m_thisMutex) {
+ return m_enabled;
+ }
+ }
+
+ /**
+ * Check the motors to see if any have timed out.
+ *
+ * <p>This static method is called periodically to poll all the motors and stop any that have
+ * timed out.
+ */
+ public static void checkMotors() {
+ synchronized (m_listMutex) {
+ for (MotorSafety elem : m_instanceList) {
+ elem.check();
+ }
+ }
+ }
+
+ public abstract void stopMotor();
+
+ public abstract String getDescription();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
new file mode 100644
index 0000000..9d1011b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Nidec Brushless Motor.
+ */
+public class NidecBrushless extends MotorSafety implements SpeedController, Sendable,
+ AutoCloseable {
+ private boolean m_isInverted;
+ private final DigitalOutput m_dio;
+ private final PWM m_pwm;
+ private volatile double m_speed;
+ private volatile boolean m_disabled;
+
+ /**
+ * Constructor.
+ *
+ * @param pwmChannel The PWM channel that the Nidec Brushless controller is attached to.
+ * 0-9 are on-board, 10-19 are on the MXP port
+ * @param dioChannel The DIO channel that the Nidec Brushless controller is attached to.
+ * 0-9 are on-board, 10-25 are on the MXP port
+ */
+ public NidecBrushless(final int pwmChannel, final int dioChannel) {
+ setSafetyEnabled(false);
+
+ // the dio controls the output (in PWM mode)
+ m_dio = new DigitalOutput(dioChannel);
+ SendableRegistry.addChild(this, m_dio);
+ m_dio.setPWMRate(15625);
+ m_dio.enablePWM(0.5);
+
+ // the pwm enables the controller
+ m_pwm = new PWM(pwmChannel);
+ SendableRegistry.addChild(this, m_pwm);
+
+ HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
+ SendableRegistry.addLW(this, "Nidec Brushless", pwmChannel);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ m_dio.close();
+ m_pwm.close();
+ }
+
+ /**
+ * Set the PWM value.
+ *
+ * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+ * FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+ @Override
+ public void set(double speed) {
+ if (!m_disabled) {
+ m_speed = speed;
+ m_dio.updateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
+ m_pwm.setRaw(0xffff);
+ }
+
+ feed();
+ }
+
+ /**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+ @Override
+ public double get() {
+ return m_speed;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ /**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+
+ /**
+ * Stop the motor. This is called by the MotorSafety object when it has a timeout for this PWM and
+ * needs to stop it from running. Calling set() will re-enable the motor.
+ */
+ @Override
+ public void stopMotor() {
+ m_dio.updateDutyCycle(0.5);
+ m_pwm.setDisabled();
+ }
+
+ @Override
+ public String getDescription() {
+ return "Nidec " + getChannel();
+ }
+
+ /**
+ * Disable the motor. The enable() function must be called to re-enable
+ * the motor.
+ */
+ @Override
+ public void disable() {
+ m_disabled = true;
+ m_dio.updateDutyCycle(0.5);
+ m_pwm.setDisabled();
+ }
+
+ /**
+ * Re-enable the motor after disable() has been called. The set()
+ * function must be called to set a new motor speed.
+ */
+ public void enable() {
+ m_disabled = false;
+ }
+
+ /**
+ * Gets the channel number associated with the object.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_pwm.getChannel();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Nidec Brushless");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
new file mode 100644
index 0000000..806bfa9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.NotifierJNI;
+
+import static java.util.Objects.requireNonNull;
+
+public class Notifier implements AutoCloseable {
+ // The thread waiting on the HAL alarm.
+ private Thread m_thread;
+ // The lock for the process information.
+ private final ReentrantLock m_processLock = new ReentrantLock();
+ // The C pointer to the notifier object. We don't use it directly, it is
+ // just passed to the JNI bindings.
+ private final AtomicInteger m_notifier = new AtomicInteger();
+ // The time, in microseconds, at which the corresponding handler should be
+ // called. Has the same zero as Utility.getFPGATime().
+ private double m_expirationTime;
+ // The handler passed in by the user which should be called at the
+ // appropriate interval.
+ private Runnable m_handler;
+ // Whether we are calling the handler just once or periodically.
+ private boolean m_periodic;
+ // If periodic, the period of the calling; if just once, stores how long it
+ // is until we call the handler.
+ private double m_period;
+
+ @Override
+ @SuppressWarnings("NoFinalizer")
+ protected void finalize() {
+ close();
+ }
+
+ @Override
+ public void close() {
+ int handle = m_notifier.getAndSet(0);
+ if (handle == 0) {
+ return;
+ }
+ NotifierJNI.stopNotifier(handle);
+ // Join the thread to ensure the handler has exited.
+ if (m_thread.isAlive()) {
+ try {
+ m_thread.interrupt();
+ m_thread.join();
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ }
+ NotifierJNI.cleanNotifier(handle);
+ m_thread = null;
+ }
+
+ /**
+ * Update the alarm hardware to reflect the next alarm.
+ *
+ * @param triggerTime the time at which the next alarm will be triggered
+ */
+ private void updateAlarm(long triggerTime) {
+ int notifier = m_notifier.get();
+ if (notifier == 0) {
+ return;
+ }
+ NotifierJNI.updateNotifierAlarm(notifier, triggerTime);
+ }
+
+ /**
+ * Update the alarm hardware to reflect the next alarm.
+ */
+ private void updateAlarm() {
+ updateAlarm((long) (m_expirationTime * 1e6));
+ }
+
+ /**
+ * Create a Notifier for timer event notification.
+ *
+ * @param run The handler that is called at the notification time which is set
+ * using StartSingle or StartPeriodic.
+ */
+ public Notifier(Runnable run) {
+ requireNonNull(run);
+
+ m_handler = run;
+ m_notifier.set(NotifierJNI.initializeNotifier());
+
+ m_thread = new Thread(() -> {
+ while (!Thread.interrupted()) {
+ int notifier = m_notifier.get();
+ if (notifier == 0) {
+ break;
+ }
+ long curTime = NotifierJNI.waitForNotifierAlarm(notifier);
+ if (curTime == 0) {
+ break;
+ }
+
+ Runnable handler = null;
+ m_processLock.lock();
+ try {
+ handler = m_handler;
+ if (m_periodic) {
+ m_expirationTime += m_period;
+ updateAlarm();
+ } else {
+ // need to update the alarm to cause it to wait again
+ updateAlarm((long) -1);
+ }
+ } finally {
+ m_processLock.unlock();
+ }
+
+ if (handler != null) {
+ handler.run();
+ }
+ }
+ });
+ m_thread.setName("Notifier");
+ m_thread.setDaemon(true);
+ m_thread.setUncaughtExceptionHandler((thread, error) -> {
+ Throwable cause = error.getCause();
+ if (cause != null) {
+ error = cause;
+ }
+ DriverStation.reportError("Unhandled exception: " + error.toString(), error.getStackTrace());
+ DriverStation.reportError(
+ "The loopFunc() method (or methods called by it) should have handled "
+ + "the exception above.", false);
+ });
+ m_thread.start();
+ }
+
+ /**
+ * Sets the name of the notifier. Used for debugging purposes only.
+ *
+ * @param name Name
+ */
+ public void setName(String name) {
+ m_thread.setName(name);
+ NotifierJNI.setNotifierName(m_notifier.get(), name);
+ }
+
+ /**
+ * Change the handler function.
+ *
+ * @param handler Handler
+ */
+ public void setHandler(Runnable handler) {
+ m_processLock.lock();
+ try {
+ m_handler = handler;
+ } finally {
+ m_processLock.unlock();
+ }
+ }
+
+ /**
+ * Register for single event notification. A timer event is queued for a single
+ * event after the specified delay.
+ *
+ * @param delay Seconds to wait before the handler is called.
+ */
+ public void startSingle(double delay) {
+ m_processLock.lock();
+ try {
+ m_periodic = false;
+ m_period = delay;
+ m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
+ updateAlarm();
+ } finally {
+ m_processLock.unlock();
+ }
+ }
+
+ /**
+ * Register for periodic event notification. A timer event is queued for
+ * periodic event notification. Each time the interrupt occurs, the event will
+ * be immediately requeued for the same time interval.
+ *
+ * @param period Period in seconds to call the handler starting one period after
+ * the call to this method.
+ */
+ public void startPeriodic(double period) {
+ m_processLock.lock();
+ try {
+ m_periodic = true;
+ m_period = period;
+ m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
+ updateAlarm();
+ } finally {
+ m_processLock.unlock();
+ }
+ }
+
+ /**
+ * Stop timer events from occurring. Stop any repeating timer events from
+ * occurring. This will also remove any single notification events from the
+ * queue. If a timer-based call to the registered handler is in progress, this
+ * function will block until the handler call is complete.
+ */
+ public void stop() {
+ NotifierJNI.cancelNotifierAlarm(m_notifier.get());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
new file mode 100644
index 0000000..40eb42f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
@@ -0,0 +1,829 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.BoundaryException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
+ * calculations, as well as writing the given PIDOutput.
+ *
+ * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
+ * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
+ * given set of PID constants.
+ *
+ * @deprecated All APIs which use this have been deprecated.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+@SuppressWarnings("PMD.TooManyFields")
+public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
+ public static final double kDefaultPeriod = 0.05;
+ private static int instances;
+
+ // Factor for "proportional" control
+ @SuppressWarnings("MemberName")
+ private double m_P;
+
+ // Factor for "integral" control
+ @SuppressWarnings("MemberName")
+ private double m_I;
+
+ // Factor for "derivative" control
+ @SuppressWarnings("MemberName")
+ private double m_D;
+
+ // Factor for "feed forward" control
+ @SuppressWarnings("MemberName")
+ private double m_F;
+
+ // |maximum output|
+ private double m_maximumOutput = 1.0;
+
+ // |minimum output|
+ private double m_minimumOutput = -1.0;
+
+ // Maximum input - limit setpoint to this
+ private double m_maximumInput;
+
+ // Minimum input - limit setpoint to this
+ private double m_minimumInput;
+
+ // Input range - difference between maximum and minimum
+ private double m_inputRange;
+
+ // Do the endpoints wrap around? (e.g., absolute encoder)
+ private boolean m_continuous;
+
+ // Is the PID controller enabled
+ protected boolean m_enabled;
+
+ // The prior error (used to compute velocity)
+ private double m_prevError;
+
+ // The sum of the errors for use in the integral calc
+ private double m_totalError;
+
+ // The tolerance object used to check if on target
+ private Tolerance m_tolerance;
+
+ private double m_setpoint;
+ private double m_prevSetpoint;
+ @SuppressWarnings("PMD.UnusedPrivateField")
+ private double m_error;
+ private double m_result;
+
+ private LinearFilter m_filter;
+
+ protected ReentrantLock m_thisMutex = new ReentrantLock();
+
+ // Ensures when disable() is called, pidWrite() won't run if calculate()
+ // is already running at that time.
+ protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
+
+ protected PIDSource m_pidInput;
+ protected PIDOutput m_pidOutput;
+ protected Timer m_setpointTimer;
+
+ /**
+ * Tolerance is the type of tolerance used to specify if the PID controller is on target.
+ *
+ * <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
+ * specify types of tolerance specifications to use.
+ */
+ public interface Tolerance {
+ boolean onTarget();
+ }
+
+ /**
+ * Used internally for when Tolerance hasn't been set.
+ */
+ public static class NullTolerance implements Tolerance {
+ @Override
+ public boolean onTarget() {
+ throw new IllegalStateException("No tolerance value set when calling onTarget().");
+ }
+ }
+
+ public class PercentageTolerance implements Tolerance {
+ private final double m_percentage;
+
+ PercentageTolerance(double value) {
+ m_percentage = value;
+ }
+
+ @Override
+ public boolean onTarget() {
+ return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
+ }
+ }
+
+ public class AbsoluteTolerance implements Tolerance {
+ private final double m_value;
+
+ AbsoluteTolerance(double value) {
+ m_value = value;
+ }
+
+ @Override
+ public boolean onTarget() {
+ return Math.abs(getError()) < m_value;
+ }
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, and F.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param Kf the feed forward term
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source,
+ PIDOutput output) {
+ requireNonNullParam(source, "PIDSource", "PIDBase");
+ requireNonNullParam(output, "output", "PIDBase");
+
+ m_setpointTimer = new Timer();
+ m_setpointTimer.start();
+
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ m_pidInput = source;
+ m_filter = LinearFilter.movingAverage(1);
+
+ m_pidOutput = output;
+
+ instances++;
+ HAL.report(tResourceType.kResourceType_PIDController, instances);
+ m_tolerance = new NullTolerance();
+ SendableRegistry.add(this, "PIDController", instances);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, and D.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source the PIDSource object that is used to get values
+ * @param output the PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+ this(Kp, Ki, Kd, 0.0, source, output);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Read the input, calculate the output accordingly, and write to the output. This should only be
+ * called by the PIDTask and is created during initialization.
+ */
+ @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.NPathComplexity"})
+ protected void calculate() {
+ if (m_pidInput == null || m_pidOutput == null) {
+ return;
+ }
+
+ boolean enabled;
+
+ m_thisMutex.lock();
+ try {
+ enabled = m_enabled;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ if (enabled) {
+ double input;
+
+ // Storage for function inputs
+ PIDSourceType pidSourceType;
+ double P;
+ double I;
+ double D;
+ double feedForward = calculateFeedForward();
+ double minimumOutput;
+ double maximumOutput;
+
+ // Storage for function input-outputs
+ double prevError;
+ double error;
+ double totalError;
+
+ m_thisMutex.lock();
+ try {
+ input = m_filter.calculate(m_pidInput.pidGet());
+
+ pidSourceType = m_pidInput.getPIDSourceType();
+ P = m_P;
+ I = m_I;
+ D = m_D;
+ minimumOutput = m_minimumOutput;
+ maximumOutput = m_maximumOutput;
+
+ prevError = m_prevError;
+ error = getContinuousError(m_setpoint - input);
+ totalError = m_totalError;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ // Storage for function outputs
+ double result;
+
+ if (pidSourceType.equals(PIDSourceType.kRate)) {
+ if (P != 0) {
+ totalError = clamp(totalError + error, minimumOutput / P,
+ maximumOutput / P);
+ }
+
+ result = P * totalError + D * error + feedForward;
+ } else {
+ if (I != 0) {
+ totalError = clamp(totalError + error, minimumOutput / I,
+ maximumOutput / I);
+ }
+
+ result = P * error + I * totalError + D * (error - prevError)
+ + feedForward;
+ }
+
+ result = clamp(result, minimumOutput, maximumOutput);
+
+ // Ensures m_enabled check and pidWrite() call occur atomically
+ m_pidWriteMutex.lock();
+ try {
+ m_thisMutex.lock();
+ try {
+ if (m_enabled) {
+ // Don't block other PIDController operations on pidWrite()
+ m_thisMutex.unlock();
+
+ m_pidOutput.pidWrite(result);
+ }
+ } finally {
+ if (m_thisMutex.isHeldByCurrentThread()) {
+ m_thisMutex.unlock();
+ }
+ }
+ } finally {
+ m_pidWriteMutex.unlock();
+ }
+
+ m_thisMutex.lock();
+ try {
+ m_prevError = error;
+ m_error = error;
+ m_totalError = totalError;
+ m_result = result;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+ }
+
+ /**
+ * Calculate the feed forward term.
+ *
+ * <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
+ * feed forward calculation is desired, the user can override this function and provide his or
+ * her own. This function does no synchronization because the PIDController class only calls it
+ * in synchronized code, so be careful if calling it oneself.
+ *
+ * <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
+ * setpoint for the output. If a position PID controller is being used, the F term should be set
+ * to 1 over the maximum speed for the output measured in setpoint units per this controller's
+ * update period (see the default period in this class's constructor).
+ */
+ protected double calculateFeedForward() {
+ if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
+ return m_F * getSetpoint();
+ } else {
+ double temp = m_F * getDeltaSetpoint();
+ m_prevSetpoint = m_setpoint;
+ m_setpointTimer.reset();
+ return temp;
+ }
+ }
+
+ /**
+ * Set the PID Controller gain parameters. Set the proportional, integral, and differential
+ * coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ */
+ @Override
+ @SuppressWarnings("ParameterName")
+ public void setPID(double p, double i, double d) {
+ m_thisMutex.lock();
+ try {
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the PID Controller gain parameters. Set the proportional, integral, and differential
+ * coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ * @param f Feed forward coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setPID(double p, double i, double d, double f) {
+ m_thisMutex.lock();
+ try {
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ m_F = f;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Proportional coefficient of the PID controller gain.
+ *
+ * @param p Proportional coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setP(double p) {
+ m_thisMutex.lock();
+ try {
+ m_P = p;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Integral coefficient of the PID controller gain.
+ *
+ * @param i Integral coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setI(double i) {
+ m_thisMutex.lock();
+ try {
+ m_I = i;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Differential coefficient of the PID controller gain.
+ *
+ * @param d differential coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setD(double d) {
+ m_thisMutex.lock();
+ try {
+ m_D = d;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Feed forward coefficient of the PID controller gain.
+ *
+ * @param f feed forward coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setF(double f) {
+ m_thisMutex.lock();
+ try {
+ m_F = f;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+ @Override
+ public double getP() {
+ m_thisMutex.lock();
+ try {
+ return m_P;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Integral coefficient.
+ *
+ * @return integral coefficient
+ */
+ @Override
+ public double getI() {
+ m_thisMutex.lock();
+ try {
+ return m_I;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Differential coefficient.
+ *
+ * @return differential coefficient
+ */
+ @Override
+ public double getD() {
+ m_thisMutex.lock();
+ try {
+ return m_D;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Feed forward coefficient.
+ *
+ * @return feed forward coefficient
+ */
+ public double getF() {
+ m_thisMutex.lock();
+ try {
+ return m_F;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Return the current PID result This is always centered on zero and constrained the the max and
+ * min outs.
+ *
+ * @return the latest calculated output
+ */
+ public double get() {
+ m_thisMutex.lock();
+ try {
+ return m_result;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the PID controller to consider the input to be continuous, Rather then using the max and
+ * min input range as constraints, it considers them to be the same point and automatically
+ * calculates the shortest route to the setpoint.
+ *
+ * @param continuous Set to true turns on continuous, false turns off continuous
+ */
+ public void setContinuous(boolean continuous) {
+ if (continuous && m_inputRange <= 0) {
+ throw new IllegalStateException("No input range set when calling setContinuous().");
+ }
+ m_thisMutex.lock();
+ try {
+ m_continuous = continuous;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the PID controller to consider the input to be continuous, Rather then using the max and
+ * min input range as constraints, it considers them to be the same point and automatically
+ * calculates the shortest route to the setpoint.
+ */
+ public void setContinuous() {
+ setContinuous(true);
+ }
+
+ /**
+ * Sets the maximum and minimum values expected from the input and setpoint.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the input
+ */
+ public void setInputRange(double minimumInput, double maximumInput) {
+ m_thisMutex.lock();
+ try {
+ if (minimumInput > maximumInput) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ setSetpoint(m_setpoint);
+ }
+
+ /**
+ * Sets the minimum and maximum values to write.
+ *
+ * @param minimumOutput the minimum percentage to write to the output
+ * @param maximumOutput the maximum percentage to write to the output
+ */
+ public void setOutputRange(double minimumOutput, double maximumOutput) {
+ m_thisMutex.lock();
+ try {
+ if (minimumOutput > maximumOutput) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the setpoint for the PIDController.
+ *
+ * @param setpoint the desired setpoint
+ */
+ @Override
+ public void setSetpoint(double setpoint) {
+ m_thisMutex.lock();
+ try {
+ if (m_maximumInput > m_minimumInput) {
+ if (setpoint > m_maximumInput) {
+ m_setpoint = m_maximumInput;
+ } else if (setpoint < m_minimumInput) {
+ m_setpoint = m_minimumInput;
+ } else {
+ m_setpoint = setpoint;
+ }
+ } else {
+ m_setpoint = setpoint;
+ }
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the current setpoint of the PIDController.
+ *
+ * @return the current setpoint
+ */
+ @Override
+ public double getSetpoint() {
+ m_thisMutex.lock();
+ try {
+ return m_setpoint;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the change in setpoint over time of the PIDController.
+ *
+ * @return the change in setpoint over time
+ */
+ public double getDeltaSetpoint() {
+ m_thisMutex.lock();
+ try {
+ return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the current difference of the input from the setpoint.
+ *
+ * @return the current error
+ */
+ @Override
+ public double getError() {
+ m_thisMutex.lock();
+ try {
+ return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the current difference of the error over the past few iterations. You can specify the
+ * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
+ * used for the onTarget() function.
+ *
+ * @deprecated Use getError(), which is now already filtered.
+ * @return the current average of the error
+ */
+ @Deprecated
+ public double getAvgError() {
+ m_thisMutex.lock();
+ try {
+ return getError();
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Sets what type of input the PID controller will use.
+ *
+ * @param pidSource the type of input
+ */
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidInput.setPIDSourceType(pidSource);
+ }
+
+ /**
+ * Returns the type of input the PID controller is using.
+ *
+ * @return the PID controller input type
+ */
+ public PIDSourceType getPIDSourceType() {
+ return m_pidInput.getPIDSourceType();
+ }
+
+ /**
+ * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
+ * the range or as an absolute value. The Tolerance object encapsulates those options in an
+ * object. Use it by creating the type of tolerance that you want to use: setTolerance(new
+ * PIDController.AbsoluteTolerance(0.1))
+ *
+ * @deprecated Use setPercentTolerance() instead.
+ * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
+ * AbsoluteTolerance
+ */
+ @Deprecated
+ public void setTolerance(Tolerance tolerance) {
+ m_tolerance = tolerance;
+ }
+
+ /**
+ * Set the absolute error which is considered tolerable for use with OnTarget.
+ *
+ * @param absvalue absolute error which is tolerable in the units of the input object
+ */
+ public void setAbsoluteTolerance(double absvalue) {
+ m_thisMutex.lock();
+ try {
+ m_tolerance = new AbsoluteTolerance(absvalue);
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
+ * 15 percent)
+ *
+ * @param percentage percent error which is tolerable
+ */
+ public void setPercentTolerance(double percentage) {
+ m_thisMutex.lock();
+ try {
+ m_tolerance = new PercentageTolerance(percentage);
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the number of previous error samples to average for tolerancing. When determining whether a
+ * mechanism is on target, the user may want to use a rolling average of previous measurements
+ * instead of a precise position or velocity. This is useful for noisy sensors which return a few
+ * erroneous measurements when the mechanism is on target. However, the mechanism will not
+ * register as on target for at least the specified bufLength cycles.
+ *
+ * @deprecated Use a LinearFilter as the input.
+ * @param bufLength Number of previous cycles to average.
+ */
+ @Deprecated
+ public void setToleranceBuffer(int bufLength) {
+ m_thisMutex.lock();
+ try {
+ m_filter = LinearFilter.movingAverage(bufLength);
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Return true if the error is within the percentage of the total input range, determined by
+ * setTolerance. This assumes that the maximum and minimum input were set using setInput.
+ *
+ * @return true if the error is less than the tolerance
+ */
+ public boolean onTarget() {
+ m_thisMutex.lock();
+ try {
+ return m_tolerance.onTarget();
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Reset the previous error, the integral term, and disable the controller.
+ */
+ @Override
+ public void reset() {
+ m_thisMutex.lock();
+ try {
+ m_prevError = 0;
+ m_totalError = 0;
+ m_result = 0;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Passes the output directly to setSetpoint().
+ *
+ * <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
+ * In that case, the output of the parent controller becomes the input (i.e., the reference) of
+ * the child.
+ *
+ * <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
+ */
+ @Override
+ public void pidWrite(double output) {
+ setSetpoint(output);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PIDController");
+ builder.setSafeState(this::reset);
+ builder.addDoubleProperty("p", this::getP, this::setP);
+ builder.addDoubleProperty("i", this::getI, this::setI);
+ builder.addDoubleProperty("d", this::getD, this::setD);
+ builder.addDoubleProperty("f", this::getF, this::setF);
+ builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+ }
+
+ /**
+ * Wraps error around for continuous inputs. The original error is returned if continuous mode is
+ * disabled. This is an unsynchronized function.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+ protected double getContinuousError(double error) {
+ if (m_continuous && m_inputRange > 0) {
+ error %= m_inputRange;
+ if (Math.abs(error) > m_inputRange / 2) {
+ if (error > 0) {
+ return error - m_inputRange;
+ } else {
+ return error + m_inputRange;
+ }
+ }
+ }
+
+ return error;
+ }
+
+ private static double clamp(double value, double low, double high) {
+ return Math.max(low, Math.min(value, high));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
new file mode 100644
index 0000000..881723e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
+ * calculations, as well as writing the given PIDOutput.
+ *
+ * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
+ * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
+ * given set of PID constants.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.controller.PIDController} instead.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public class PIDController extends PIDBase implements Controller, AutoCloseable {
+ Notifier m_controlLoop = new Notifier(this::calculate);
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, and F.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param Kf the feed forward term
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ * @param period the loop time for doing calculations in seconds.
+ * This particularly affects calculations of
+ * the integral and differential terms.
+ * The default is 0.05 (50ms).
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source,
+ PIDOutput output, double period) {
+ super(Kp, Ki, Kd, Kf, source, output);
+ m_controlLoop.startPeriodic(period);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D and period.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source the PIDSource object that is used to get values
+ * @param output the PIDOutput object that is set to the output percentage
+ * @param period the loop time for doing calculations in seconds.
+ * This particularly affects calculations of
+ * the integral and differential terms.
+ * The default is 0.05 (50ms).
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output,
+ double period) {
+ this(Kp, Ki, Kd, 0.0, source, output, period);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+ this(Kp, Ki, Kd, source, output, kDefaultPeriod);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param Kf the feed forward term
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source,
+ PIDOutput output) {
+ this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
+ }
+
+ @Override
+ public void close() {
+ m_controlLoop.close();
+ m_thisMutex.lock();
+ try {
+ m_pidOutput = null;
+ m_pidInput = null;
+ m_controlLoop = null;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Begin running the PIDController.
+ */
+ @Override
+ public void enable() {
+ m_thisMutex.lock();
+ try {
+ m_enabled = true;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Stop running the PIDController, this sets the output to zero before stopping.
+ */
+ @Override
+ public void disable() {
+ // Ensures m_enabled check and pidWrite() call occur atomically
+ m_pidWriteMutex.lock();
+ try {
+ m_thisMutex.lock();
+ try {
+ m_enabled = false;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ m_pidOutput.pidWrite(0);
+ } finally {
+ m_pidWriteMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the enabled state of the PIDController.
+ */
+ public void setEnabled(boolean enable) {
+ if (enable) {
+ enable();
+ } else {
+ disable();
+ }
+ }
+
+ /**
+ * Return true if PIDController is enabled.
+ */
+ public boolean isEnabled() {
+ m_thisMutex.lock();
+ try {
+ return m_enabled;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Reset the previous error, the integral term, and disable the controller.
+ */
+ @Override
+ public void reset() {
+ disable();
+
+ super.reset();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ super.initSendable(builder);
+ builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
new file mode 100644
index 0000000..10e60d3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+@Deprecated(since = "2020", forRemoval = true)
+@SuppressWarnings("SummaryJavadoc")
+public interface PIDInterface {
+ @SuppressWarnings("ParameterName")
+ void setPID(double p, double i, double d);
+
+ double getP();
+
+ double getI();
+
+ double getD();
+
+ void setSetpoint(double setpoint);
+
+ double getSetpoint();
+
+ double getError();
+
+ void reset();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
new file mode 100644
index 0000000..e9af2ff
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows PIDController to write it's results to its output.
+ *
+ * @deprecated Use DoubleConsumer and new PIDController class.
+ */
+@FunctionalInterface
+@Deprecated(since = "2020", forRemoval = true)
+public interface PIDOutput {
+ /**
+ * Set the output to the value calculated by PIDController.
+ *
+ * @param output the value calculated by PIDController
+ */
+ void pidWrite(double output);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
new file mode 100644
index 0000000..3c4f1f5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows for PIDController to automatically read from this object.
+ *
+ * @deprecated Use DoubleSupplier and new PIDController class.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public interface PIDSource {
+ /**
+ * Set which parameter of the device you are using as a process control variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ void setPIDSourceType(PIDSourceType pidSource);
+
+ /**
+ * Get which parameter of the device you are using as a process control variable.
+ *
+ * @return the currently selected PID source parameter
+ */
+ PIDSourceType getPIDSourceType();
+
+ /**
+ * Get the result to use in PIDController.
+ *
+ * @return the result to use in PIDController
+ */
+ double pidGet();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
new file mode 100644
index 0000000..7508be5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * A description for the type of output value to provide to a PIDController.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public enum PIDSourceType {
+ kDisplacement,
+ kRate
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
new file mode 100644
index 0000000..8febd2c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
@@ -0,0 +1,265 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PWMConfigDataResult;
+import edu.wpi.first.hal.PWMJNI;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class implements the PWM generation in the FPGA.
+ *
+ * <p>The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They are mapped to
+ * the hardware dependent values, in this case 0-2000 for the FPGA. Changes are immediately sent to
+ * the FPGA, and the update occurs at the next FPGA cycle (5.005ms). There is no delay.
+ *
+ * <p>As of revision 0.1.10 of the FPGA, the FPGA interprets the 0-2000 values as follows: - 2000 =
+ * maximum pulse width - 1999 to 1001 = linear scaling from "full forward" to "center" - 1000 =
+ * center value - 999 to 2 = linear scaling from "center" to "full reverse" - 1 = minimum pulse
+ * width (currently .5ms) - 0 = disabled (i.e. PWM output is held low)
+ */
+public class PWM extends MotorSafety implements Sendable, AutoCloseable {
+ /**
+ * Represents the amount to multiply the minimum servo-pulse pwm period by.
+ */
+ public enum PeriodMultiplier {
+ /**
+ * Period Multiplier: don't skip pulses. PWM pulses occur every 5.005 ms
+ */
+ k1X,
+ /**
+ * Period Multiplier: skip every other pulse. PWM pulses occur every 10.010 ms
+ */
+ k2X,
+ /**
+ * Period Multiplier: skip three out of four pulses. PWM pulses occur every 20.020 ms
+ */
+ k4X
+ }
+
+ private final int m_channel;
+
+ // Package private to use from AddressableLED
+ int m_handle;
+
+ /**
+ * Allocate a PWM given a channel.
+ *
+ * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+ */
+ public PWM(final int channel) {
+ SensorUtil.checkPWMChannel(channel);
+ m_channel = channel;
+
+ m_handle = PWMJNI.initializePWMPort(HAL.getPort((byte) channel));
+
+ setDisabled();
+
+ PWMJNI.setPWMEliminateDeadband(m_handle, false);
+
+ HAL.report(tResourceType.kResourceType_PWM, channel + 1);
+ SendableRegistry.addLW(this, "PWM", channel);
+
+ setSafetyEnabled(false);
+ }
+
+ /**
+ * Free the resource associated with the PWM channel and set the value to 0.
+ */
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ if (m_handle == 0) {
+ return;
+ }
+ setDisabled();
+ PWMJNI.freePWMPort(m_handle);
+ m_handle = 0;
+ }
+
+ @Override
+ public void stopMotor() {
+ setDisabled();
+ }
+
+ @Override
+ public String getDescription() {
+ return "PWM " + getChannel();
+ }
+
+ /**
+ * Optionally eliminate the deadband from a speed controller.
+ *
+ * @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate the deadband
+ * in the middle of the range. Otherwise, keep the full range without
+ * modifying any values.
+ */
+ public void enableDeadbandElimination(boolean eliminateDeadband) {
+ PWMJNI.setPWMEliminateDeadband(m_handle, eliminateDeadband);
+ }
+
+ /**
+ * Set the bounds on the PWM pulse widths. This sets the bounds on the PWM values for a particular
+ * type of controller. The values determine the upper and lower speeds as well as the deadband
+ * bracket.
+ *
+ * @param max The max PWM pulse width in ms
+ * @param deadbandMax The high end of the deadband range pulse width in ms
+ * @param center The center (off) pulse width in ms
+ * @param deadbandMin The low end of the deadband pulse width in ms
+ * @param min The minimum pulse width in ms
+ */
+ public void setBounds(double max, double deadbandMax, double center, double deadbandMin,
+ double min) {
+ PWMJNI.setPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min);
+ }
+
+ /**
+ * Gets the bounds on the PWM pulse widths. This Gets the bounds on the PWM values for a
+ * particular type of controller. The values determine the upper and lower speeds as well
+ * as the deadband bracket.
+ */
+ public PWMConfigDataResult getRawBounds() {
+ return PWMJNI.getPWMConfigRaw(m_handle);
+ }
+
+ /**
+ * Gets the channel number associated with the PWM Object.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Set the PWM value based on a position.
+ *
+ * <p>This is intended to be used by servos.
+ *
+ * @param pos The position to set the servo between 0.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public void setPosition(double pos) {
+ PWMJNI.setPWMPosition(m_handle, pos);
+ }
+
+ /**
+ * Get the PWM value in terms of a position.
+ *
+ * <p>This is intended to be used by servos.
+ *
+ * @return The position the servo is set to between 0.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public double getPosition() {
+ return PWMJNI.getPWMPosition(m_handle);
+ }
+
+ /**
+ * Set the PWM value based on a speed.
+ *
+ * <p>This is intended to be used by speed controllers.
+ *
+ * @param speed The speed to set the speed controller between -1.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetCenterPwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public void setSpeed(double speed) {
+ PWMJNI.setPWMSpeed(m_handle, speed);
+ }
+
+ /**
+ * Get the PWM value in terms of speed.
+ *
+ * <p>This is intended to be used by speed controllers.
+ *
+ * @return The most recently set speed between -1.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public double getSpeed() {
+ return PWMJNI.getPWMSpeed(m_handle);
+ }
+
+ /**
+ * Set the PWM value directly to the hardware.
+ *
+ * <p>Write a raw value to a PWM channel.
+ *
+ * @param value Raw PWM value. Range 0 - 255.
+ */
+ public void setRaw(int value) {
+ PWMJNI.setPWMRaw(m_handle, (short) value);
+ }
+
+ /**
+ * Get the PWM value directly from the hardware.
+ *
+ * <p>Read a raw value from a PWM channel.
+ *
+ * @return Raw PWM control value. Range: 0 - 255.
+ */
+ public int getRaw() {
+ return PWMJNI.getPWMRaw(m_handle);
+ }
+
+ /**
+ * Temporarily disables the PWM output. The next set call will reenable
+ * the output.
+ */
+ public void setDisabled() {
+ PWMJNI.setPWMDisabled(m_handle);
+ }
+
+ /**
+ * Slow down the PWM signal for old devices.
+ *
+ * @param mult The period multiplier to apply to this channel
+ */
+ public void setPeriodMultiplier(PeriodMultiplier mult) {
+ switch (mult) {
+ case k4X:
+ // Squelch 3 out of 4 outputs
+ PWMJNI.setPWMPeriodScale(m_handle, 3);
+ break;
+ case k2X:
+ // Squelch 1 out of 2 outputs
+ PWMJNI.setPWMPeriodScale(m_handle, 1);
+ break;
+ case k1X:
+ // Don't squelch any outputs
+ PWMJNI.setPWMPeriodScale(m_handle, 0);
+ break;
+ default:
+ // Cannot hit this, limited by PeriodMultiplier enum
+ }
+ }
+
+ protected void setZeroLatch() {
+ PWMJNI.latchPWMZero(m_handle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PWM");
+ builder.setActuator(true);
+ builder.setSafeState(this::setDisabled);
+ builder.addDoubleProperty("Value", this::getRaw, value -> setRaw((int) value));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
new file mode 100644
index 0000000..0f97ea5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * REV Robotics SPARK MAX Speed Controller with PWM control.
+ *
+ * <P>Note that the SPARK MAX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the SPARK MAX User Manual
+ * available from REV Robotics.
+ *
+ * <p><ul>
+ * <li> 2.003ms = full "forward"
+ * <li> 1.550ms = the "high end" of the deadband range
+ * <li> 1.500ms = center of the deadband range (off)
+ * <li> 1.460ms = the "low end" of the deadband range
+ * <li> 0.999ms = full "reverse"
+ * </ul>
+ */
+public class PWMSparkMax extends PWMSpeedController {
+ /**
+ * Common initialization code called by all constructors.
+ *
+ */
+ public PWMSparkMax(final int channel) {
+ super(channel);
+
+ setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMSparkMax", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
new file mode 100644
index 0000000..0a33423
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Common base class for all PWM Speed Controllers.
+ */
+public abstract class PWMSpeedController extends PWM implements SpeedController {
+ private boolean m_isInverted;
+
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ protected PWMSpeedController(int channel) {
+ super(channel);
+ }
+
+ @Override
+ public String getDescription() {
+ return "PWM " + getChannel();
+ }
+
+ /**
+ * Set the PWM value.
+ *
+ * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+ * FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+ @Override
+ public void set(double speed) {
+ setSpeed(m_isInverted ? -speed : speed);
+ feed();
+ }
+
+ /**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+ @Override
+ public double get() {
+ return getSpeed();
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ @Override
+ public void disable() {
+ setDisabled();
+ }
+
+ /**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Speed Controller");
+ builder.setActuator(true);
+ builder.setSafeState(this::setDisabled);
+ builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
new file mode 100644
index 0000000..03acab2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM control.
+ *
+ * <p>Note that the TalonFX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the TalonFX User Manual
+ * available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonFX extends PWMSpeedController {
+ /**
+ * Constructor for a TalonFX connected via PWM.
+ *
+ * @param channel The PWM channel that the Talon FX is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ public PWMTalonFX(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_TalonFX, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMTalonFX", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
new file mode 100644
index 0000000..1859e7f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control.
+ *
+ * <p>Note that the TalonSRX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the TalonSRX User Manual
+ * available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonSRX extends PWMSpeedController {
+ /**
+ * Constructor for a TalonSRX connected via PWM.
+ *
+ * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ public PWMTalonSRX(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMTalonSRX", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
new file mode 100644
index 0000000..9a1116d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * <p>Note that the Venom uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVenom extends PWMSpeedController {
+ /**
+ * Constructor for a Venom connected via PWM.
+ *
+ * @param channel The PWM channel that the Venom is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ public PWMVenom(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_FusionVenom, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMVenom", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
new file mode 100644
index 0000000..b51686a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM control.
+ *
+ * <p>Note that the Victor SPX uses the following bounds for PWM values. These values should
+ * work reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the Victor SPX User
+ * Manual available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVictorSPX extends PWMSpeedController {
+ /**
+ * Constructor for a Victor SPX connected via PWM.
+ *
+ * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9 are on-board, 10-19
+ * are on the MXP port
+ */
+ public PWMVictorSPX(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel() + 1);
+ SendableRegistry.setName(this, "PWMVictorSPX", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
new file mode 100644
index 0000000..e30be1b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PDPJNI;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the Power Distribution
+ * Panel over CAN.
+ */
+public class PowerDistributionPanel implements Sendable, AutoCloseable {
+ private final int m_handle;
+
+ /**
+ * Constructor.
+ *
+ * @param module The CAN ID of the PDP
+ */
+ public PowerDistributionPanel(int module) {
+ SensorUtil.checkPDPModule(module);
+ m_handle = PDPJNI.initializePDP(module);
+
+ HAL.report(tResourceType.kResourceType_PDP, module + 1);
+ SendableRegistry.addLW(this, "PowerDistributionPanel", module);
+ }
+
+ /**
+ * Constructor. Uses the default CAN ID (0).
+ */
+ public PowerDistributionPanel() {
+ this(0);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Query the input voltage of the PDP.
+ *
+ * @return The voltage of the PDP in volts
+ */
+ public double getVoltage() {
+ return PDPJNI.getPDPVoltage(m_handle);
+ }
+
+ /**
+ * Query the temperature of the PDP.
+ *
+ * @return The temperature of the PDP in degrees Celsius
+ */
+ public double getTemperature() {
+ return PDPJNI.getPDPTemperature(m_handle);
+ }
+
+ /**
+ * Query the current of a single channel of the PDP.
+ *
+ * @return The current of one of the PDP channels (channels 0-15) in Amperes
+ */
+ public double getCurrent(int channel) {
+ double current = PDPJNI.getPDPChannelCurrent((byte) channel, m_handle);
+
+ SensorUtil.checkPDPChannel(channel);
+
+ return current;
+ }
+
+ /**
+ * Query the current of all monitored PDP channels (0-15).
+ *
+ * @return The current of all the channels in Amperes
+ */
+ public double getTotalCurrent() {
+ return PDPJNI.getPDPTotalCurrent(m_handle);
+ }
+
+ /**
+ * Query the total power drawn from the monitored PDP channels.
+ *
+ * @return the total power in Watts
+ */
+ public double getTotalPower() {
+ return PDPJNI.getPDPTotalPower(m_handle);
+ }
+
+ /**
+ * Query the total energy drawn from the monitored PDP channels.
+ *
+ * @return the total energy in Joules
+ */
+ public double getTotalEnergy() {
+ return PDPJNI.getPDPTotalEnergy(m_handle);
+ }
+
+ /**
+ * Reset the total energy to 0.
+ */
+ public void resetTotalEnergy() {
+ PDPJNI.resetPDPTotalEnergy(m_handle);
+ }
+
+ /**
+ * Clear all PDP sticky faults.
+ */
+ public void clearStickyFaults() {
+ PDPJNI.clearPDPStickyFaults(m_handle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PowerDistributionPanel");
+ for (int i = 0; i < SensorUtil.kPDPChannels; ++i) {
+ final int chan = i;
+ builder.addDoubleProperty("Chan" + i, () -> getCurrent(chan), null);
+ }
+ builder.addDoubleProperty("Voltage", this::getVoltage, null);
+ builder.addDoubleProperty("TotalCurrent", this::getTotalCurrent, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
new file mode 100644
index 0000000..b25d907
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
@@ -0,0 +1,258 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Collection;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * The preferences class provides a relatively simple way to save important values to the roboRIO to
+ * access the next time the roboRIO is booted.
+ *
+ * <p> This class loads and saves from a file inside the roboRIO. The user can not access the file
+ * directly, but may modify values at specific fields which will then be automatically saved to the
+ * file by the NetworkTables server. </p>
+ *
+ * <p> This class is thread safe. </p>
+ *
+ * <p> This will also interact with {@link NetworkTable} by creating a table called "Preferences"
+ * with all the key-value pairs. </p>
+ */
+public final class Preferences {
+ /**
+ * The Preferences table name.
+ */
+ private static final String TABLE_NAME = "Preferences";
+ /**
+ * The singleton instance.
+ */
+ private static Preferences instance;
+ /**
+ * The network table.
+ */
+ private final NetworkTable m_table;
+
+ /**
+ * Returns the preferences instance.
+ *
+ * @return the preferences instance
+ */
+ public static synchronized Preferences getInstance() {
+ if (instance == null) {
+ instance = new Preferences();
+ }
+ return instance;
+ }
+
+ /**
+ * Creates a preference class.
+ */
+ private Preferences() {
+ m_table = NetworkTableInstance.getDefault().getTable(TABLE_NAME);
+ m_table.getEntry(".type").setString("RobotPreferences");
+ // Listener to set all Preferences values to persistent
+ // (for backwards compatibility with old dashboards).
+ m_table.addEntryListener(
+ (table, key, entry, value, flags) -> entry.setPersistent(),
+ EntryListenerFlags.kImmediate | EntryListenerFlags.kNew);
+ HAL.report(tResourceType.kResourceType_Preferences, 0);
+ }
+
+ /**
+ * Gets the preferences keys.
+ * @return a collection of the keys
+ */
+ public Collection<String> getKeys() {
+ return m_table.getKeys();
+ }
+
+ /**
+ * Puts the given string into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ * @throws NullPointerException if value is null
+ */
+ public void putString(String key, String value) {
+ requireNonNullParam(value, "value", "putString");
+
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setString(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given int into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putInt(String key, int value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given double into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putDouble(String key, double value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given float into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putFloat(String key, float value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given boolean into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putBoolean(String key, boolean value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setBoolean(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given long into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putLong(String key, long value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Returns whether or not there is a key with the given name.
+ *
+ * @param key the key
+ * @return if there is a value at the given key
+ */
+ public boolean containsKey(String key) {
+ return m_table.containsKey(key);
+ }
+
+ /**
+ * Remove a preference.
+ *
+ * @param key the key
+ */
+ public void remove(String key) {
+ m_table.delete(key);
+ }
+
+ /**
+ * Remove all preferences.
+ */
+ public void removeAll() {
+ for (String key : m_table.getKeys()) {
+ if (!".type".equals(key)) {
+ remove(key);
+ }
+ }
+ }
+
+ /**
+ * Returns the string at the given key. If this table does not have a value for that position,
+ * then the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public String getString(String key, String backup) {
+ return m_table.getEntry(key).getString(backup);
+ }
+
+ /**
+ * Returns the int at the given key. If this table does not have a value for that position, then
+ * the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public int getInt(String key, int backup) {
+ return (int) m_table.getEntry(key).getDouble(backup);
+ }
+
+ /**
+ * Returns the double at the given key. If this table does not have a value for that position,
+ * then the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public double getDouble(String key, double backup) {
+ return m_table.getEntry(key).getDouble(backup);
+ }
+
+ /**
+ * Returns the boolean at the given key. If this table does not have a value for that position,
+ * then the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public boolean getBoolean(String key, boolean backup) {
+ return m_table.getEntry(key).getBoolean(backup);
+ }
+
+ /**
+ * Returns the float at the given key. If this table does not have a value for that position, then
+ * the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public float getFloat(String key, float backup) {
+ return (float) m_table.getEntry(key).getDouble(backup);
+ }
+
+ /**
+ * Returns the long at the given key. If this table does not have a value for that position, then
+ * the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public long getLong(String key, long backup) {
+ return (long) m_table.getEntry(key).getDouble(backup);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
new file mode 100644
index 0000000..fde4ef1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -0,0 +1,315 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Arrays;
+import java.util.Optional;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.RelayJNI;
+import edu.wpi.first.hal.util.UncleanStatusException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * Class for VEX Robotics Spike style relay outputs. Relays are intended to be connected to Spikes
+ * or similar relays. The relay channels controls a pair of channels that are either both off, one
+ * on, the other on, or both on. This translates into two Spike outputs at 0v, one at 12v and one
+ * at 0v, one at 0v and the other at 12v, or two Spike outputs at 12V. This allows off, full
+ * forward, or full reverse control of motors without variable speed. It also allows the two
+ * channels (forward and reverse) to be used independently for something that does not care about
+ * voltage polarity (like a solenoid).
+ */
+public class Relay extends MotorSafety implements Sendable, AutoCloseable {
+ /**
+ * This class represents errors in trying to set relay values contradictory to the direction to
+ * which the relay is set.
+ */
+ public static class InvalidValueException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ public InvalidValueException(String message) {
+ super(message);
+ }
+ }
+
+ /**
+ * The state to drive a Relay to.
+ */
+ public enum Value {
+ kOff("Off"),
+ kOn("On"),
+ kForward("Forward"),
+ kReverse("Reverse");
+
+ private final String m_prettyValue;
+
+ Value(String prettyValue) {
+ m_prettyValue = prettyValue;
+ }
+
+ public String getPrettyValue() {
+ return m_prettyValue;
+ }
+
+ public static Optional<Value> getValueOf(String value) {
+ return Arrays.stream(Value.values()).filter(v -> v.m_prettyValue.equals(value)).findFirst();
+ }
+ }
+
+ /**
+ * The Direction(s) that a relay is configured to operate in.
+ */
+ public enum Direction {
+ /**
+ * direction: both directions are valid.
+ */
+
+ kBoth,
+ /**
+ * direction: Only forward is valid.
+ */
+ kForward,
+ /**
+ * direction: only reverse is valid.
+ */
+ kReverse
+ }
+
+ private final int m_channel;
+
+ private int m_forwardHandle;
+ private int m_reverseHandle;
+
+ private Direction m_direction;
+
+ /**
+ * Common relay initialization method. This code is common to all Relay constructors and
+ * initializes the relay and reserves all resources that need to be locked. Initially the relay is
+ * set to both lines at 0v.
+ */
+ private void initRelay() {
+ SensorUtil.checkRelayChannel(m_channel);
+
+ int portHandle = HAL.getPort((byte) m_channel);
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ m_forwardHandle = RelayJNI.initializeRelayPort(portHandle, true);
+ HAL.report(tResourceType.kResourceType_Relay, m_channel + 1);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ m_reverseHandle = RelayJNI.initializeRelayPort(portHandle, false);
+ HAL.report(tResourceType.kResourceType_Relay, m_channel + 128);
+ }
+
+ setSafetyEnabled(false);
+
+ SendableRegistry.addLW(this, "Relay", m_channel);
+ }
+
+ /**
+ * Relay constructor given a channel.
+ *
+ * @param channel The channel number for this relay (0 - 3).
+ * @param direction The direction that the Relay object will control.
+ */
+ public Relay(final int channel, Direction direction) {
+ m_channel = channel;
+ m_direction = requireNonNullParam(direction, "direction", "Relay");
+ initRelay();
+ set(Value.kOff);
+ }
+
+ /**
+ * Relay constructor given a channel, allowing both directions.
+ *
+ * @param channel The channel number for this relay (0 - 3).
+ */
+ public Relay(final int channel) {
+ this(channel, Direction.kBoth);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ freeRelay();
+ }
+
+ private void freeRelay() {
+ try {
+ RelayJNI.setRelay(m_forwardHandle, false);
+ } catch (UncleanStatusException ignored) {
+ // do nothing. Ignore
+ }
+ try {
+ RelayJNI.setRelay(m_reverseHandle, false);
+ } catch (UncleanStatusException ignored) {
+ // do nothing. Ignore
+ }
+
+ RelayJNI.freeRelayPort(m_forwardHandle);
+ RelayJNI.freeRelayPort(m_reverseHandle);
+
+ m_forwardHandle = 0;
+ m_reverseHandle = 0;
+ }
+
+ /**
+ * Set the relay state.
+ *
+ * <p>Valid values depend on which directions of the relay are controlled by the object.
+ *
+ * <p>When set to kBothDirections, the relay can be set to any of the four states: 0v-0v, 12v-0v,
+ * 0v-12v, 12v-12v
+ *
+ * <p>When set to kForwardOnly or kReverseOnly, you can specify the constant for the direction or
+ * you can simply specify kOff and kOn. Using only kOff and kOn is recommended.
+ *
+ * @param value The state to set the relay.
+ */
+ @SuppressWarnings("PMD.CyclomaticComplexity")
+ public void set(Value value) {
+ switch (value) {
+ case kOff:
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ RelayJNI.setRelay(m_forwardHandle, false);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ RelayJNI.setRelay(m_reverseHandle, false);
+ }
+ break;
+ case kOn:
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ RelayJNI.setRelay(m_forwardHandle, true);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ RelayJNI.setRelay(m_reverseHandle, true);
+ }
+ break;
+ case kForward:
+ if (m_direction == Direction.kReverse) {
+ throw new InvalidValueException("A relay configured for reverse cannot be set to "
+ + "forward");
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ RelayJNI.setRelay(m_forwardHandle, true);
+ }
+ if (m_direction == Direction.kBoth) {
+ RelayJNI.setRelay(m_reverseHandle, false);
+ }
+ break;
+ case kReverse:
+ if (m_direction == Direction.kForward) {
+ throw new InvalidValueException("A relay configured for forward cannot be set to "
+ + "reverse");
+ }
+ if (m_direction == Direction.kBoth) {
+ RelayJNI.setRelay(m_forwardHandle, false);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ RelayJNI.setRelay(m_reverseHandle, true);
+ }
+ break;
+ default:
+ // Cannot hit this, limited by Value enum
+ }
+ }
+
+ /**
+ * Get the Relay State.
+ *
+ * <p>Gets the current state of the relay.
+ *
+ * <p>When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
+ * kForward/kReverse (per the recommendation in Set)
+ *
+ * @return The current state of the relay as a Relay::Value
+ */
+ public Value get() {
+ if (m_direction == Direction.kForward) {
+ if (RelayJNI.getRelay(m_forwardHandle)) {
+ return Value.kOn;
+ } else {
+ return Value.kOff;
+ }
+ } else if (m_direction == Direction.kReverse) {
+ if (RelayJNI.getRelay(m_reverseHandle)) {
+ return Value.kOn;
+ } else {
+ return Value.kOff;
+ }
+ } else {
+ if (RelayJNI.getRelay(m_forwardHandle)) {
+ if (RelayJNI.getRelay(m_reverseHandle)) {
+ return Value.kOn;
+ } else {
+ return Value.kForward;
+ }
+ } else {
+ if (RelayJNI.getRelay(m_reverseHandle)) {
+ return Value.kReverse;
+ } else {
+ return Value.kOff;
+ }
+ }
+ }
+ }
+
+ /**
+ * Get the channel number.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ @Override
+ public void stopMotor() {
+ set(Value.kOff);
+ }
+
+ @Override
+ public String getDescription() {
+ return "Relay ID " + getChannel();
+ }
+
+ /**
+ * Set the Relay Direction.
+ *
+ * <p>Changes which values the relay can be set to depending on which direction is used
+ *
+ * <p>Valid inputs are kBothDirections, kForwardOnly, and kReverseOnly
+ *
+ * @param direction The direction for the relay to operate in
+ */
+ public void setDirection(Direction direction) {
+ requireNonNullParam(direction, "direction", "setDirection");
+ if (m_direction == direction) {
+ return;
+ }
+
+ freeRelay();
+ m_direction = direction;
+ initRelay();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Relay");
+ builder.setActuator(true);
+ builder.setSafeState(() -> set(Value.kOff));
+ builder.addStringProperty("Value", () -> get().getPrettyValue(),
+ value -> set(Value.getValueOf(value).orElse(Value.kOff)));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
new file mode 100644
index 0000000..39a0eac
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.hal.util.CheckedAllocationException;
+
+/**
+ * Track resources in the program. The Resource class is a convenient way of keeping track of
+ * allocated arbitrary resources in the program. Resources are just indices that have an lower and
+ * upper bound that are tracked by this class. In the library they are used for tracking allocation
+ * of hardware channels but this is purely arbitrary. The resource class does not do any actual
+ * allocation, but simply tracks if a given index is currently in use.
+ *
+ * <p><b>WARNING:</b> this should only be statically allocated. When the program loads into memory
+ * all the static constructors are called. At that time a linked list of all the "Resources" is
+ * created. Then when the program actually starts - in the Robot constructor, all resources are
+ * initialized. This ensures that the program is restartable in memory without having to
+ * unload/reload.
+ */
+public final class Resource {
+ private static Resource resourceList;
+ private final boolean[] m_numAllocated;
+ private final int m_size;
+ private final Resource m_nextResource;
+
+ /**
+ * Clears all allocated resources.
+ */
+ public static void restartProgram() {
+ for (Resource r = Resource.resourceList; r != null; r = r.m_nextResource) {
+ for (int i = 0; i < r.m_size; i++) {
+ r.m_numAllocated[i] = false;
+ }
+ }
+ }
+
+ /**
+ * Allocate storage for a new instance of Resource. Allocate a bool array of values that will get
+ * initialized to indicate that no resources have been allocated yet. The indices of the
+ * resources are 0..size-1.
+ *
+ * @param size The number of blocks to allocate
+ */
+ public Resource(final int size) {
+ m_size = size;
+ m_numAllocated = new boolean[size];
+ for (int i = 0; i < size; i++) {
+ m_numAllocated[i] = false;
+ }
+ m_nextResource = Resource.resourceList;
+ Resource.resourceList = this;
+ }
+
+ /**
+ * Allocate a resource. When a resource is requested, mark it allocated. In this case, a free
+ * resource value within the range is located and returned after it is marked allocated.
+ *
+ * @return The index of the allocated block.
+ * @throws CheckedAllocationException If there are no resources available to be allocated.
+ */
+ public int allocate() throws CheckedAllocationException {
+ for (int i = 0; i < m_size; i++) {
+ if (!m_numAllocated[i]) {
+ m_numAllocated[i] = true;
+ return i;
+ }
+ }
+ throw new CheckedAllocationException("No available resources");
+ }
+
+ /**
+ * Allocate a specific resource value. The user requests a specific resource value, i.e. channel
+ * number and it is verified unallocated, then returned.
+ *
+ * @param index The resource to allocate
+ * @return The index of the allocated block
+ * @throws CheckedAllocationException If there are no resources available to be allocated.
+ */
+ public int allocate(final int index) throws CheckedAllocationException {
+ if (index >= m_size || index < 0) {
+ throw new CheckedAllocationException("Index " + index + " out of range");
+ }
+ if (m_numAllocated[index]) {
+ throw new CheckedAllocationException("Resource at index " + index + " already allocated");
+ }
+ m_numAllocated[index] = true;
+ return index;
+ }
+
+ /**
+ * Free an allocated resource. After a resource is no longer needed, for example a destructor is
+ * called for a channel assignment class, Free will release the resource value so it can be reused
+ * somewhere else in the program.
+ *
+ * @param index The index of the resource to free.
+ */
+ public void free(final int index) {
+ if (!m_numAllocated[index]) {
+ throw new AllocationException("No resource available to be freed");
+ }
+ m_numAllocated[index] = false;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
new file mode 100644
index 0000000..e2861f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
+
+import edu.wpi.cscore.CameraServerJNI;
+import edu.wpi.first.cameraserver.CameraServerShared;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.util.WPILibVersion;
+
+/**
+ * Implement a Robot Program framework. The RobotBase class is intended to be subclassed by a user
+ * creating a robot program. Overridden autonomous() and operatorControl() methods are called at the
+ * appropriate time as the match proceeds. In the current implementation, the Autonomous code will
+ * run to completion before the OperatorControl code could start. In the future the Autonomous code
+ * might be spawned as a task, then killed at the end of the Autonomous period.
+ */
+public abstract class RobotBase implements AutoCloseable {
+ /**
+ * The ID of the main Java thread.
+ */
+ // This is usually 1, but it is best to make sure
+ private static long m_threadId = -1;
+
+ private static void setupCameraServerShared() {
+ CameraServerShared shared = new CameraServerShared() {
+
+ @Override
+ public void reportVideoServer(int id) {
+ HAL.report(tResourceType.kResourceType_PCVideoServer, id + 1);
+ }
+
+ @Override
+ public void reportUsbCamera(int id) {
+ HAL.report(tResourceType.kResourceType_UsbCamera, id + 1);
+ }
+
+ @Override
+ public void reportDriverStationError(String error) {
+ DriverStation.reportError(error, true);
+ }
+
+ @Override
+ public void reportAxisCamera(int id) {
+ HAL.report(tResourceType.kResourceType_AxisCamera, id + 1);
+ }
+
+ @Override
+ public Long getRobotMainThreadId() {
+ return RobotBase.getMainThreadId();
+ }
+
+ @Override
+ public boolean isRoboRIO() {
+ return RobotBase.isReal();
+ }
+ };
+
+ CameraServerSharedStore.setCameraServerShared(shared);
+ }
+
+ protected final DriverStation m_ds;
+
+ /**
+ * Constructor for a generic robot program. User code should be placed in the constructor that
+ * runs before the Autonomous or Operator Control period starts. The constructor will run to
+ * completion before Autonomous is entered.
+ *
+ * <p>This must be used to ensure that the communications code starts. In the future it would be
+ * nice
+ * to put this code into it's own task that loads on boot so ensure that it runs.
+ */
+ protected RobotBase() {
+ NetworkTableInstance inst = NetworkTableInstance.getDefault();
+ m_threadId = Thread.currentThread().getId();
+ setupCameraServerShared();
+ inst.setNetworkIdentity("Robot");
+ if (isReal()) {
+ inst.startServer("/home/lvuser/networktables.ini");
+ } else {
+ inst.startServer();
+ }
+ m_ds = DriverStation.getInstance();
+ inst.getTable("LiveWindow").getSubTable(".status").getEntry("LW Enabled").setBoolean(false);
+
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ }
+
+ public static long getMainThreadId() {
+ return m_threadId;
+ }
+
+ @Override
+ public void close() {
+ }
+
+ /**
+ * Get if the robot is a simulation.
+ *
+ * @return If the robot is running in simulation.
+ */
+ public static boolean isSimulation() {
+ return !isReal();
+ }
+
+ /**
+ * Get if the robot is real.
+ *
+ * @return If the robot is running in the real world.
+ */
+ public static boolean isReal() {
+ return HALUtil.getHALRuntimeType() == 0;
+ }
+
+ /**
+ * Determine if the Robot is currently disabled.
+ *
+ * @return True if the Robot is currently disabled by the field controls.
+ */
+ public boolean isDisabled() {
+ return m_ds.isDisabled();
+ }
+
+ /**
+ * Determine if the Robot is currently enabled.
+ *
+ * @return True if the Robot is currently enabled by the field controls.
+ */
+ public boolean isEnabled() {
+ return m_ds.isEnabled();
+ }
+
+ /**
+ * Determine if the robot is currently in Autonomous mode as determined by the field
+ * controls.
+ *
+ * @return True if the robot is currently operating Autonomously.
+ */
+ public boolean isAutonomous() {
+ return m_ds.isAutonomous();
+ }
+
+ /**
+ * Determine if the robot is currently in Test mode as determined by the driver
+ * station.
+ *
+ * @return True if the robot is currently operating in Test mode.
+ */
+ public boolean isTest() {
+ return m_ds.isTest();
+ }
+
+ /**
+ * Determine if the robot is currently in Operator Control mode as determined by the field
+ * controls.
+ *
+ * @return True if the robot is currently operating in Tele-Op mode.
+ */
+ public boolean isOperatorControl() {
+ return m_ds.isOperatorControl();
+ }
+
+ /**
+ * Indicates if new data is available from the driver station.
+ *
+ * @return Has new data arrived over the network since the last time this function was called?
+ */
+ public boolean isNewDataAvailable() {
+ return m_ds.isNewControlData();
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ public abstract void startCompetition();
+
+ /**
+ * Ends the main loop in startCompetition().
+ */
+ public abstract void endCompetition();
+
+ @SuppressWarnings("JavadocMethod")
+ public static boolean getBooleanProperty(String name, boolean defaultValue) {
+ String propVal = System.getProperty(name);
+ if (propVal == null) {
+ return defaultValue;
+ }
+ if ("false".equalsIgnoreCase(propVal)) {
+ return false;
+ } else if ("true".equalsIgnoreCase(propVal)) {
+ return true;
+ } else {
+ throw new IllegalStateException(propVal);
+ }
+ }
+
+ private static final ReentrantLock m_runMutex = new ReentrantLock();
+ private static RobotBase m_robotCopy;
+ private static boolean m_suppressExitWarning;
+
+ /**
+ * Run the robot main loop.
+ */
+ @SuppressWarnings({"PMD.AvoidInstantiatingObjectsInLoops", "PMD.AvoidCatchingThrowable",
+ "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
+ System.out.println("********** Robot program starting **********");
+
+ T robot;
+ try {
+ robot = robotSupplier.get();
+ } catch (Throwable throwable) {
+ Throwable cause = throwable.getCause();
+ if (cause != null) {
+ throwable = cause;
+ }
+ String robotName = "Unknown";
+ StackTraceElement[] elements = throwable.getStackTrace();
+ if (elements.length > 0) {
+ robotName = elements[0].getClassName();
+ }
+ DriverStation.reportError("Unhandled exception instantiating robot " + robotName + " "
+ + throwable.toString(), elements);
+ DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+ DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
+ return;
+ }
+
+ m_runMutex.lock();
+ m_robotCopy = robot;
+ m_runMutex.unlock();
+
+ if (isReal()) {
+ try {
+ final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini");
+
+ if (file.exists()) {
+ file.delete();
+ }
+
+ file.createNewFile();
+
+ try (OutputStream output = Files.newOutputStream(file.toPath())) {
+ output.write("Java ".getBytes(StandardCharsets.UTF_8));
+ output.write(WPILibVersion.Version.getBytes(StandardCharsets.UTF_8));
+ }
+
+ } catch (IOException ex) {
+ DriverStation.reportError("Could not write FRC_Lib_Version.ini: " + ex.toString(),
+ ex.getStackTrace());
+ }
+ }
+
+ boolean errorOnExit = false;
+ try {
+ robot.startCompetition();
+ } catch (Throwable throwable) {
+ Throwable cause = throwable.getCause();
+ if (cause != null) {
+ throwable = cause;
+ }
+ DriverStation.reportError("Unhandled exception: " + throwable.toString(),
+ throwable.getStackTrace());
+ errorOnExit = true;
+ } finally {
+ m_runMutex.lock();
+ boolean suppressExitWarning = m_suppressExitWarning;
+ m_runMutex.unlock();
+ if (!suppressExitWarning) {
+ // startCompetition never returns unless exception occurs....
+ DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+ if (errorOnExit) {
+ DriverStation.reportError(
+ "The startCompetition() method (or methods called by it) should have "
+ + "handled the exception above.", false);
+ } else {
+ DriverStation.reportError("Unexpected return from startCompetition() method.", false);
+ }
+ }
+ }
+ }
+
+ /**
+ * Suppress the "Robots should not quit" message.
+ */
+ public static void suppressExitWarning(boolean value) {
+ m_runMutex.lock();
+ m_suppressExitWarning = value;
+ m_runMutex.unlock();
+ }
+
+ /**
+ * Starting point for the applications.
+ */
+ public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
+ if (!HAL.initialize(500, 0)) {
+ throw new IllegalStateException("Failed to initialize. Terminating");
+ }
+
+ // Call a CameraServer JNI function to force OpenCV native library loading
+ // Needed because all the OpenCV JNI functions don't have built in loading
+ CameraServerJNI.enumerateSinks();
+
+ HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Java, 0,
+ WPILibVersion.Version);
+
+ if (HAL.hasMain()) {
+ Thread thread = new Thread(() -> {
+ runRobot(robotSupplier);
+ HAL.exitMain();
+ }, "robot main");
+ thread.setDaemon(true);
+ thread.start();
+ HAL.runMain();
+ suppressExitWarning(true);
+ m_runMutex.lock();
+ RobotBase robot = m_robotCopy;
+ m_runMutex.unlock();
+ if (robot != null) {
+ robot.endCompetition();
+ }
+ try {
+ thread.join(1000);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ } else {
+ runRobot(robotSupplier);
+ }
+
+ System.exit(1);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
new file mode 100644
index 0000000..0623812
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.hal.PowerJNI;
+import edu.wpi.first.hal.can.CANJNI;
+import edu.wpi.first.hal.can.CANStatus;
+
+/**
+ * Contains functions for roboRIO functionality.
+ */
+public final class RobotController {
+ private RobotController() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Return the FPGA Version number. For now, expect this to be the current
+ * year.
+ *
+ * @return FPGA Version number.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static int getFPGAVersion() {
+ return HALUtil.getFPGAVersion();
+ }
+
+ /**
+ * Return the FPGA Revision number. The format of the revision is 3 numbers. The 12 most
+ * significant bits are the Major Revision. the next 8 bits are the Minor Revision. The 12 least
+ * significant bits are the Build Number.
+ *
+ * @return FPGA Revision number.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static long getFPGARevision() {
+ return (long) HALUtil.getFPGARevision();
+ }
+
+ /**
+ * Read the microsecond timer from the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA.
+ */
+ public static long getFPGATime() {
+ return HALUtil.getFPGATime();
+ }
+
+ /**
+ * Get the state of the "USER" button on the roboRIO.
+ *
+ * @return true if the button is currently pressed down
+ */
+ public static boolean getUserButton() {
+ return HALUtil.getFPGAButton();
+ }
+
+ /**
+ * Read the battery voltage.
+ *
+ * @return The battery voltage in Volts.
+ */
+ public static double getBatteryVoltage() {
+ return PowerJNI.getVinVoltage();
+ }
+
+ /**
+ * Gets a value indicating whether the FPGA outputs are enabled. The outputs may be disabled if
+ * the robot is disabled or e-stopped, the watchdog has expired, or if the roboRIO browns out.
+ *
+ * @return True if the FPGA outputs are enabled.
+ */
+ public static boolean isSysActive() {
+ return HAL.getSystemActive();
+ }
+
+ /**
+ * Check if the system is browned out.
+ *
+ * @return True if the system is browned out
+ */
+ public static boolean isBrownedOut() {
+ return HAL.getBrownedOut();
+ }
+
+ /**
+ * Get the input voltage to the robot controller.
+ *
+ * @return The controller input voltage value in Volts
+ */
+ public static double getInputVoltage() {
+ return PowerJNI.getVinVoltage();
+ }
+
+ /**
+ * Get the input current to the robot controller.
+ *
+ * @return The controller input current value in Amps
+ */
+ public static double getInputCurrent() {
+ return PowerJNI.getVinCurrent();
+ }
+
+ /**
+ * Get the voltage of the 3.3V rail.
+ *
+ * @return The controller 3.3V rail voltage value in Volts
+ */
+ public static double getVoltage3V3() {
+ return PowerJNI.getUserVoltage3V3();
+ }
+
+ /**
+ * Get the current output of the 3.3V rail.
+ *
+ * @return The controller 3.3V rail output current value in Volts
+ */
+ public static double getCurrent3V3() {
+ return PowerJNI.getUserCurrent3V3();
+ }
+
+ /**
+ * Get the enabled state of the 3.3V rail. The rail may be disabled due to a controller brownout,
+ * a short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 3.3V rail enabled value
+ */
+ public static boolean getEnabled3V3() {
+ return PowerJNI.getUserActive3V3();
+ }
+
+ /**
+ * Get the count of the total current faults on the 3.3V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ public static int getFaultCount3V3() {
+ return PowerJNI.getUserCurrentFaults3V3();
+ }
+
+ /**
+ * Get the voltage of the 5V rail.
+ *
+ * @return The controller 5V rail voltage value in Volts
+ */
+ public static double getVoltage5V() {
+ return PowerJNI.getUserVoltage5V();
+ }
+
+ /**
+ * Get the current output of the 5V rail.
+ *
+ * @return The controller 5V rail output current value in Amps
+ */
+ public static double getCurrent5V() {
+ return PowerJNI.getUserCurrent5V();
+ }
+
+ /**
+ * Get the enabled state of the 5V rail. The rail may be disabled due to a controller brownout, a
+ * short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 5V rail enabled value
+ */
+ public static boolean getEnabled5V() {
+ return PowerJNI.getUserActive5V();
+ }
+
+ /**
+ * Get the count of the total current faults on the 5V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ public static int getFaultCount5V() {
+ return PowerJNI.getUserCurrentFaults5V();
+ }
+
+ /**
+ * Get the voltage of the 6V rail.
+ *
+ * @return The controller 6V rail voltage value in Volts
+ */
+ public static double getVoltage6V() {
+ return PowerJNI.getUserVoltage6V();
+ }
+
+ /**
+ * Get the current output of the 6V rail.
+ *
+ * @return The controller 6V rail output current value in Amps
+ */
+ public static double getCurrent6V() {
+ return PowerJNI.getUserCurrent6V();
+ }
+
+ /**
+ * Get the enabled state of the 6V rail. The rail may be disabled due to a controller brownout, a
+ * short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 6V rail enabled value
+ */
+ public static boolean getEnabled6V() {
+ return PowerJNI.getUserActive6V();
+ }
+
+ /**
+ * Get the count of the total current faults on the 6V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ public static int getFaultCount6V() {
+ return PowerJNI.getUserCurrentFaults6V();
+ }
+
+ /**
+ * Get the current status of the CAN bus.
+ *
+ * @return The status of the CAN bus
+ */
+ public static CANStatus getCANStatus() {
+ CANStatus status = new CANStatus();
+ CANJNI.GetCANStatus(status);
+ return status;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
new file mode 100644
index 0000000..104fb26
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
@@ -0,0 +1,716 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 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) or with the Tank/Arcade
+ * functions intended to be used for Operator Control driving.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.drive.DifferentialDrive}
+ * or {@link edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
+ */
+@Deprecated
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+public class RobotDrive extends MotorSafety implements AutoCloseable {
+ /**
+ * The location of a motor on the robot for the purpose of driving.
+ */
+ public enum MotorType {
+ kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ MotorType(int value) {
+ this.value = value;
+ }
+ }
+
+ public static final double kDefaultExpirationTime = 0.1;
+ public static final double kDefaultSensitivity = 0.5;
+ public static final double kDefaultMaxOutput = 1.0;
+ protected static final int kMaxNumberOfMotors = 4;
+ protected double m_sensitivity;
+ protected double m_maxOutput;
+ protected SpeedController m_frontLeftMotor;
+ protected SpeedController m_frontRightMotor;
+ protected SpeedController m_rearLeftMotor;
+ protected SpeedController m_rearRightMotor;
+ protected boolean m_allocatedSpeedControllers;
+ protected static boolean kArcadeRatioCurve_Reported;
+ protected static boolean kTank_Reported;
+ protected static boolean kArcadeStandard_Reported;
+ protected static boolean kMecanumCartesian_Reported;
+ protected static boolean kMecanumPolar_Reported;
+
+ /**
+ * Constructor for RobotDrive with 2 motors specified with channel numbers. Set up parameters for
+ * a two wheel drive system where the left and right motor pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ *
+ * @param leftMotorChannel The PWM channel number that drives the left motor.
+ * @param rightMotorChannel The PWM channel number that drives the right motor.
+ */
+ public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_frontLeftMotor = null;
+ m_rearLeftMotor = new Talon(leftMotorChannel);
+ m_frontRightMotor = null;
+ m_rearRightMotor = new Talon(rightMotorChannel);
+ m_allocatedSpeedControllers = true;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Constructor for RobotDrive with 4 motors specified with channel numbers. Set up parameters for
+ * a four wheel drive system where all four motor pwm channels are specified in the call. This
+ * call assumes Talons for controlling the motors.
+ *
+ * @param frontLeftMotor Front left motor channel number
+ * @param rearLeftMotor Rear Left motor channel number
+ * @param frontRightMotor Front right motor channel number
+ * @param rearRightMotor Rear Right motor channel number
+ */
+ public RobotDrive(final int frontLeftMotor, final int rearLeftMotor, final int frontRightMotor,
+ final int rearRightMotor) {
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_rearLeftMotor = new Talon(rearLeftMotor);
+ m_rearRightMotor = new Talon(rearRightMotor);
+ m_frontLeftMotor = new Talon(frontLeftMotor);
+ m_frontRightMotor = new Talon(frontRightMotor);
+ m_allocatedSpeedControllers = true;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Constructor for RobotDrive with 2 motors specified as SpeedController objects. The
+ * SpeedController version of the constructor enables programs to use the RobotDrive classes with
+ * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
+ * the curve to suit motor bias or dead-band elimination.
+ *
+ * @param leftMotor The left SpeedController object used to drive the robot.
+ * @param rightMotor the right SpeedController object used to drive the robot.
+ */
+ public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
+ requireNonNullParam(leftMotor, "leftMotor", "RobotDrive");
+ requireNonNullParam(rightMotor, "rightMotor", "RobotDrive");
+
+ m_frontLeftMotor = null;
+ m_rearLeftMotor = leftMotor;
+ m_frontRightMotor = null;
+ m_rearRightMotor = rightMotor;
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_allocatedSpeedControllers = false;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Constructor for RobotDrive with 4 motors specified as SpeedController objects. Speed controller
+ * input version of RobotDrive (see previous comments).
+ *
+ * @param frontLeftMotor The front left SpeedController object used to drive the robot
+ * @param rearLeftMotor The back left SpeedController object used to drive the robot.
+ * @param frontRightMotor The front right SpeedController object used to drive the robot.
+ * @param rearRightMotor The back right SpeedController object used to drive the robot.
+ */
+ public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
+ SpeedController frontRightMotor, SpeedController rearRightMotor) {
+ m_frontLeftMotor = requireNonNullParam(frontLeftMotor, "frontLeftMotor", "RobotDrive");
+ m_rearLeftMotor = requireNonNullParam(rearLeftMotor, "rearLeftMotor", "RobotDrive");
+ m_frontRightMotor = requireNonNullParam(frontRightMotor, "frontRightMotor", "RobotDrive");
+ m_rearRightMotor = requireNonNullParam(rearRightMotor, "rearRightMotor", "RobotDrive");
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_allocatedSpeedControllers = false;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Drive the motors at "outputMagnitude" and "curve". Both outputMagnitude and curve are -1.0 to
+ * +1.0 values, where 0.0 represents stopped and not turning. {@literal curve < 0 will turn left
+ * and curve > 0} will turn right.
+ *
+ * <p>The algorithm for steering provides a constant turn radius for any normal speed range, both
+ * forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
+ *
+ * <p>This function will most likely be used in an autonomous routine.
+ *
+ * @param outputMagnitude The speed setting for the outside wheel in a turn, forward or backwards,
+ * +1 to -1.
+ * @param curve The rate of turn, constant for different forward speeds. Set {@literal
+ * curve < 0 for left turn or curve > 0 for right turn.} Set curve =
+ * e^(-r/w) to get a turn radius r for wheelbase w of your robot.
+ * Conversely, turn radius r = -ln(curve)*w for a given value of curve and
+ * wheelbase w.
+ */
+ public void drive(double outputMagnitude, double curve) {
+ final double leftOutput;
+ final double rightOutput;
+
+ if (!kArcadeRatioCurve_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_ArcadeRatioCurve,
+ getNumMotors());
+ kArcadeRatioCurve_Reported = true;
+ }
+ if (curve < 0) {
+ double value = Math.log(-curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) {
+ ratio = 0.0000000001;
+ }
+ leftOutput = outputMagnitude / ratio;
+ rightOutput = outputMagnitude;
+ } else if (curve > 0) {
+ double value = Math.log(curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) {
+ ratio = 0.0000000001;
+ }
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude / ratio;
+ } else {
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude;
+ }
+ setLeftRightMotorOutputs(leftOutput, rightOutput);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. drive the robot using two joystick
+ * inputs. The Y-axis will be selected from each Joystick object. The calculated values will be
+ * squared to decrease sensitivity at low speeds.
+ *
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ */
+ public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
+ requireNonNullParam(leftStick, "leftStick", "tankDrive");
+ requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+ tankDrive(leftStick.getY(), rightStick.getY(), true);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. drive the robot using two joystick
+ * inputs. The Y-axis will be selected from each Joystick object.
+ *
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
+ requireNonNullParam(leftStick, "leftStick", "tankDrive");
+ requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+ tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you pick the
+ * axis to be used on each Joystick object for the left and right sides of the robot. The
+ * calculated values will be squared to decrease sensitivity at low speeds.
+ *
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ */
+ public void tankDrive(GenericHID leftStick, final int leftAxis, GenericHID rightStick,
+ final int rightAxis) {
+ requireNonNullParam(leftStick, "leftStick", "tankDrive");
+ requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+ tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you pick the
+ * axis to be used on each Joystick object for the left and right sides of the robot.
+ *
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void tankDrive(GenericHID leftStick, final int leftAxis, GenericHID rightStick,
+ final int rightAxis, boolean squaredInputs) {
+ requireNonNullParam(leftStick, "leftStick", "tankDrive");
+ requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+ tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you directly
+ * provide joystick values from any source.
+ *
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
+ if (!kTank_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_Tank,
+ getNumMotors());
+ kTank_Reported = true;
+ }
+
+ leftValue = limit(leftValue);
+ rightValue = limit(rightValue);
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (squaredInputs) {
+ leftValue = Math.copySign(leftValue * leftValue, leftValue);
+ rightValue = Math.copySign(rightValue * rightValue, rightValue);
+ }
+ setLeftRightMotorOutputs(leftValue, rightValue);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you directly
+ * provide joystick values from any source. The calculated values will be squared to decrease
+ * sensitivity at low speeds.
+ *
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ */
+ public void tankDrive(double leftValue, double rightValue) {
+ tankDrive(leftValue, rightValue, true);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
+ * axis for the move value and the X axis for the rotate value. (Should add more information here
+ * regarding the way that arcade drive works.)
+ *
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be
+ * selected for forwards/backwards and the X-axis will be selected for
+ * rotation rate.
+ * @param squaredInputs If true, the sensitivity will be decreased for small values
+ */
+ public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
+ // simply call the full-featured arcadeDrive with the appropriate values
+ arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
+ * axis for the move value and the X axis for the rotate value. (Should add more information here
+ * regarding the way that arcade drive works.) The calculated values will be squared to decrease
+ * sensitivity at low speeds.
+ *
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
+ * for forwards/backwards and the X-axis will be selected for rotation rate.
+ */
+ public void arcadeDrive(GenericHID stick) {
+ arcadeDrive(stick, true);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given two joystick instances and two axis,
+ * compute the values to send to either two or four motors.
+ *
+ * @param moveStick The Joystick object that represents the forward/backward direction
+ * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
+ * Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate right/left
+ * (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void arcadeDrive(GenericHID moveStick, final int moveAxis, GenericHID rotateStick,
+ final int rotateAxis, boolean squaredInputs) {
+ double moveValue = moveStick.getRawAxis(moveAxis);
+ double rotateValue = rotateStick.getRawAxis(rotateAxis);
+
+ arcadeDrive(moveValue, rotateValue, squaredInputs);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given two joystick instances and two axis,
+ * compute the values to send to either two or four motors. The calculated values will be
+ * squared to decrease sensitivity at low speeds.
+ *
+ * @param moveStick The Joystick object that represents the forward/backward direction
+ * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
+ * Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically
+ * X_AXIS)
+ */
+ public void arcadeDrive(GenericHID moveStick, final int moveAxis, GenericHID rotateStick,
+ final int rotateAxis) {
+ arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. This function lets you directly provide
+ * joystick values from any source.
+ *
+ * @param moveValue The value to use for forwards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ * @param squaredInputs If set, decreases the sensitivity at low speeds
+ */
+ public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
+ // local variables to hold the computed PWM values for the motors
+ if (!kArcadeStandard_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_ArcadeStandard,
+ getNumMotors());
+ kArcadeStandard_Reported = true;
+ }
+
+ double leftMotorSpeed;
+ double rightMotorSpeed;
+
+ moveValue = limit(moveValue);
+ rotateValue = limit(rotateValue);
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (squaredInputs) {
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ moveValue = Math.copySign(moveValue * moveValue, moveValue);
+ rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
+ }
+
+ if (moveValue > 0.0) {
+ if (rotateValue > 0.0) {
+ leftMotorSpeed = moveValue - rotateValue;
+ rightMotorSpeed = Math.max(moveValue, rotateValue);
+ } else {
+ leftMotorSpeed = Math.max(moveValue, -rotateValue);
+ rightMotorSpeed = moveValue + rotateValue;
+ }
+ } else {
+ if (rotateValue > 0.0) {
+ leftMotorSpeed = -Math.max(-moveValue, rotateValue);
+ rightMotorSpeed = moveValue + rotateValue;
+ } else {
+ leftMotorSpeed = moveValue - rotateValue;
+ rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
+ }
+ }
+
+ setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. This function lets you directly provide
+ * joystick values from any source. The calculated values will be squared to decrease
+ * sensitivity at low speeds.
+ *
+ * @param moveValue The value to use for forwards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ */
+ public void arcadeDrive(double moveValue, double rotateValue) {
+ arcadeDrive(moveValue, rotateValue, true);
+ }
+
+ /**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
+ * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
+ * top, the roller axles should form an X across the robot.
+ *
+ * <p>This is designed to be directly driven by joystick axes.
+ *
+ * @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
+ * @param y The speed that the robot should drive in the Y direction. This input is
+ * inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
+ * @param rotation The rate of rotation for the robot that is completely independent of the
+ * translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented
+ * controls.
+ */
+ @SuppressWarnings("ParameterName")
+ public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
+ if (!kMecanumCartesian_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_MecanumCartesian,
+ getNumMotors());
+ kMecanumCartesian_Reported = true;
+ }
+ @SuppressWarnings("LocalVariableName")
+ double xIn = x;
+ @SuppressWarnings("LocalVariableName")
+ double yIn = y;
+ // Negate y for the joystick.
+ yIn = -yIn;
+ // Compensate for gyro angle.
+ double[] rotated = rotateVector(xIn, yIn, gyroAngle);
+ xIn = rotated[0];
+ yIn = rotated[1];
+
+ double[] wheelSpeeds = new double[kMaxNumberOfMotors];
+ wheelSpeeds[MotorType.kFrontLeft.value] = xIn + yIn + rotation;
+ wheelSpeeds[MotorType.kFrontRight.value] = -xIn + yIn - rotation;
+ wheelSpeeds[MotorType.kRearLeft.value] = -xIn + yIn + rotation;
+ wheelSpeeds[MotorType.kRearRight.value] = xIn + yIn - rotation;
+
+ normalize(wheelSpeeds);
+ m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
+ m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
+ m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
+ m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
+ * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
+ * top, the roller axles should form an X across the robot.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
+ * @param direction The angle the robot should drive in degrees. The direction and magnitude
+ * are independent of the rotation rate. [-180.0..180.0]
+ * @param rotation The rate of rotation for the robot that is completely independent of the
+ * magnitude or direction. [-1.0..1.0]
+ */
+ public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
+ if (!kMecanumPolar_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_MecanumPolar,
+ getNumMotors());
+ kMecanumPolar_Reported = true;
+ }
+ // Normalized for full power along the Cartesian axes.
+ magnitude = limit(magnitude) * Math.sqrt(2.0);
+ // The rollers are at 45 degree angles.
+ double dirInRad = (direction + 45.0) * Math.PI / 180.0;
+ double cosD = Math.cos(dirInRad);
+ double sinD = Math.sin(dirInRad);
+
+ double[] wheelSpeeds = new double[kMaxNumberOfMotors];
+ wheelSpeeds[MotorType.kFrontLeft.value] = sinD * magnitude + rotation;
+ wheelSpeeds[MotorType.kFrontRight.value] = cosD * magnitude - rotation;
+ wheelSpeeds[MotorType.kRearLeft.value] = cosD * magnitude + rotation;
+ wheelSpeeds[MotorType.kRearRight.value] = sinD * magnitude - rotation;
+
+ normalize(wheelSpeeds);
+
+ m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
+ m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
+ m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
+ m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Holonomic Drive method for Mecanum wheeled robots.
+ *
+ * <p>This is an alias to mecanumDrive_Polar() for backward compatibility
+ *
+ * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
+ * @param direction The direction the robot should drive. The direction and maginitude are
+ * independent of the rotation rate.
+ * @param rotation The rate of rotation for the robot that is completely independent of the
+ * magnitute or direction. [-1.0..1.0]
+ */
+ void holonomicDrive(double magnitude, double direction, double rotation) {
+ mecanumDrive_Polar(magnitude, direction, rotation);
+ }
+
+ /**
+ * Set the speed of the right and left motors. This is used once an appropriate drive setup
+ * function is called such as twoWheelDrive(). The motors are set to "leftSpeed" and
+ * "rightSpeed" and includes flipping the direction of one side for opposing motors.
+ *
+ * @param leftOutput The speed to send to the left side of the robot.
+ * @param rightOutput The speed to send to the right side of the robot.
+ */
+ public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
+
+ if (m_frontLeftMotor != null) {
+ m_frontLeftMotor.set(limit(leftOutput) * m_maxOutput);
+ }
+ m_rearLeftMotor.set(limit(leftOutput) * m_maxOutput);
+
+ if (m_frontRightMotor != null) {
+ m_frontRightMotor.set(-limit(rightOutput) * m_maxOutput);
+ }
+ m_rearRightMotor.set(-limit(rightOutput) * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+ protected static double limit(double number) {
+ if (number > 1.0) {
+ return 1.0;
+ }
+ if (number < -1.0) {
+ return -1.0;
+ }
+ return number;
+ }
+
+ /**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+ protected static void normalize(double[] wheelSpeeds) {
+ double maxMagnitude = Math.abs(wheelSpeeds[0]);
+ for (int i = 1; i < kMaxNumberOfMotors; i++) {
+ double temp = Math.abs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) {
+ maxMagnitude = temp;
+ }
+ }
+ if (maxMagnitude > 1.0) {
+ for (int i = 0; i < kMaxNumberOfMotors; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+ }
+
+ /**
+ * Rotate a vector in Cartesian space.
+ */
+ @SuppressWarnings("ParameterName")
+ protected static double[] rotateVector(double x, double y, double angle) {
+ double cosA = Math.cos(angle * (Math.PI / 180.0));
+ double sinA = Math.sin(angle * (Math.PI / 180.0));
+ double[] out = new double[2];
+ out[0] = x * cosA - y * sinA;
+ out[1] = x * sinA + y * cosA;
+ return out;
+ }
+
+ /**
+ * Invert a motor direction. This is used when a motor should run in the opposite direction as the
+ * drive code would normally run it. Motors that are direct drive would be inverted, the drive
+ * code assumes that the motors are geared with one reversal.
+ *
+ * @param motor The motor index to invert.
+ * @param isInverted True if the motor should be inverted when operated.
+ */
+ public void setInvertedMotor(MotorType motor, boolean isInverted) {
+ switch (motor) {
+ case kFrontLeft:
+ m_frontLeftMotor.setInverted(isInverted);
+ break;
+ case kFrontRight:
+ m_frontRightMotor.setInverted(isInverted);
+ break;
+ case kRearLeft:
+ m_rearLeftMotor.setInverted(isInverted);
+ break;
+ case kRearRight:
+ m_rearRightMotor.setInverted(isInverted);
+ break;
+ default:
+ throw new IllegalArgumentException("Illegal motor type: " + motor);
+ }
+ }
+
+ /**
+ * Set the turning sensitivity.
+ *
+ * <p>This only impacts the drive() entry-point.
+ *
+ * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
+ */
+ public void setSensitivity(double sensitivity) {
+ m_sensitivity = sensitivity;
+ }
+
+ /**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than
+ * PercentVbus.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_maxOutput = maxOutput;
+ }
+
+ /**
+ * Free the speed controllers if they were allocated locally.
+ */
+ @Override
+ public void close() {
+ if (m_allocatedSpeedControllers) {
+ if (m_frontLeftMotor != null) {
+ ((PWM) m_frontLeftMotor).close();
+ }
+ if (m_frontRightMotor != null) {
+ ((PWM) m_frontRightMotor).close();
+ }
+ if (m_rearLeftMotor != null) {
+ ((PWM) m_rearLeftMotor).close();
+ }
+ if (m_rearRightMotor != null) {
+ ((PWM) m_rearRightMotor).close();
+ }
+ }
+ }
+
+ @Override
+ public String getDescription() {
+ return "Robot Drive";
+ }
+
+ @Override
+ public void stopMotor() {
+ if (m_frontLeftMotor != null) {
+ m_frontLeftMotor.stopMotor();
+ }
+ if (m_frontRightMotor != null) {
+ m_frontRightMotor.stopMotor();
+ }
+ if (m_rearLeftMotor != null) {
+ m_rearLeftMotor.stopMotor();
+ }
+ if (m_rearRightMotor != null) {
+ m_rearRightMotor.stopMotor();
+ }
+
+ feed();
+ }
+
+ protected int getNumMotors() {
+ int motors = 0;
+ if (m_frontLeftMotor != null) {
+ motors++;
+ }
+ if (m_frontRightMotor != null) {
+ motors++;
+ }
+ if (m_rearLeftMotor != null) {
+ motors++;
+ }
+ if (m_rearRightMotor != null) {
+ motors++;
+ }
+ return motors;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
new file mode 100644
index 0000000..58004d5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+@SuppressWarnings("JavadocMethod")
+public final class RobotState {
+ public static boolean isDisabled() {
+ return DriverStation.getInstance().isDisabled();
+ }
+
+ public static boolean isEnabled() {
+ return DriverStation.getInstance().isEnabled();
+ }
+
+ public static boolean isEStopped() {
+ return DriverStation.getInstance().isEStopped();
+ }
+
+ public static boolean isOperatorControl() {
+ return DriverStation.getInstance().isOperatorControl();
+ }
+
+ public static boolean isAutonomous() {
+ return DriverStation.getInstance().isAutonomous();
+ }
+
+ public static boolean isTest() {
+ return DriverStation.getInstance().isTest();
+ }
+
+ private RobotState() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
new file mode 100644
index 0000000..b2e98b6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Mindsensors SD540 Speed Controller.
+ *
+ * <p>Note that the SD540 uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the SD540 User Manual
+ * available from Mindsensors.
+ *
+ * <p><ul>
+ * <li>2.05ms = full "forward"
+ * <li>1.55ms = the "high end" of the deadband range
+ * <li>1.50ms = center of the deadband range (off)
+ * <li>1.44ms = the "low end" of the deadband range
+ * <li>0.94ms = full "reverse"
+ * </ul>
+ */
+public class SD540 extends PWMSpeedController {
+ /**
+ * Common initialization code called by all constructors.
+ */
+ protected void initSD540() {
+ setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
+ SendableRegistry.setName(this, "SD540", getChannel());
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public SD540(final int channel) {
+ super(channel);
+ initSD540();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
new file mode 100644
index 0000000..aca2cbe
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -0,0 +1,806 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.IntBuffer;
+
+import edu.wpi.first.hal.AccumulatorResult;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SPIJNI;
+
+/**
+ * Represents a SPI bus port.
+ */
+@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.TooManyMethods"})
+public class SPI implements AutoCloseable {
+ public enum Port {
+ kOnboardCS0(0), kOnboardCS1(1), kOnboardCS2(2), kOnboardCS3(3), kMXP(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Port(int value) {
+ this.value = value;
+ }
+ }
+
+ private int m_port;
+ private int m_msbFirst;
+ private int m_clockIdleHigh;
+ private int m_sampleOnTrailing;
+
+ /**
+ * Constructor.
+ *
+ * @param port the physical SPI port
+ */
+ public SPI(Port port) {
+ m_port = (byte) port.value;
+
+ SPIJNI.spiInitialize(m_port);
+
+ HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
+ }
+
+ @Override
+ public void close() {
+ if (m_accum != null) {
+ m_accum.close();
+ m_accum = null;
+ }
+ SPIJNI.spiClose(m_port);
+ }
+
+ /**
+ * Configure the rate of the generated clock signal. The default value is 500,000 Hz. The maximum
+ * value is 4,000,000 Hz.
+ *
+ * @param hz The clock rate in Hertz.
+ */
+ public final void setClockRate(int hz) {
+ SPIJNI.spiSetSpeed(m_port, hz);
+ }
+
+ /**
+ * Configure the order that bits are sent and received on the wire to be most significant bit
+ * first.
+ */
+ public final void setMSBFirst() {
+ m_msbFirst = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure the order that bits are sent and received on the wire to be least significant bit
+ * first.
+ */
+ public final void setLSBFirst() {
+ m_msbFirst = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure the clock output line to be active low. This is sometimes called clock polarity high
+ * or clock idle high.
+ */
+ public final void setClockActiveLow() {
+ m_clockIdleHigh = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure the clock output line to be active high. This is sometimes called clock polarity low
+ * or clock idle low.
+ */
+ public final void setClockActiveHigh() {
+ m_clockIdleHigh = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the leading edge and the data changes on the trailing
+ * edge.
+ */
+ public final void setSampleDataOnLeadingEdge() {
+ m_sampleOnTrailing = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the trailing edge and the data changes on the leading
+ * edge.
+ */
+ public final void setSampleDataOnTrailingEdge() {
+ m_sampleOnTrailing = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the falling edge and the data changes on the rising edge.
+ * Note this gets reversed is setClockActiveLow is set.
+ * @deprecated use {@link #setSampleDataOnTrailingEdge()} in most cases.
+ */
+ @Deprecated
+ public final void setSampleDataOnFalling() {
+ m_sampleOnTrailing = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the rising edge and the data changes on the falling edge.
+ * Note this gets reversed is setClockActiveLow is set.
+ * @deprecated use {@link #setSampleDataOnLeadingEdge()} in most cases.
+ */
+ @Deprecated
+ public final void setSampleDataOnRising() {
+ m_sampleOnTrailing = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+
+
+ /**
+ * Configure the chip select line to be active high.
+ */
+ public final void setChipSelectActiveHigh() {
+ SPIJNI.spiSetChipSelectActiveHigh(m_port);
+ }
+
+ /**
+ * Configure the chip select line to be active low.
+ */
+ public final void setChipSelectActiveLow() {
+ SPIJNI.spiSetChipSelectActiveLow(m_port);
+ }
+
+ /**
+ * Write data to the slave device. Blocks until there is space in the output FIFO.
+ *
+ * <p>If not running in output only mode, also saves the data received on the MISO input during
+ * the transfer into the receive FIFO.
+ */
+ public int write(byte[] dataToSend, int size) {
+ if (dataToSend.length < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiWriteB(m_port, dataToSend, (byte) size);
+ }
+
+ /**
+ * Write data to the slave device. Blocks until there is space in the output FIFO.
+ *
+ * <p>If not running in output only mode, also saves the data received on the MISO input during
+ * the transfer into the receive FIFO.
+ *
+ * @param dataToSend The buffer containing the data to send.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public int write(ByteBuffer dataToSend, int size) {
+ if (dataToSend.hasArray()) {
+ return write(dataToSend.array(), size);
+ }
+ if (!dataToSend.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (dataToSend.capacity() < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiWrite(m_port, dataToSend, (byte) size);
+ }
+
+ /**
+ * Read a word from the receive FIFO.
+ *
+ * <p>Waits for the current transfer to complete if the receive FIFO is empty.
+ *
+ * <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
+ *
+ * @param initiate If true, this function pushes "0" into the transmit buffer and initiates a
+ * transfer. If false, this function assumes that data is already in the receive
+ * FIFO from a previous write.
+ */
+ public int read(boolean initiate, byte[] dataReceived, int size) {
+ if (dataReceived.length < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiReadB(m_port, initiate, dataReceived, (byte) size);
+ }
+
+ /**
+ * Read a word from the receive FIFO.
+ *
+ * <p>Waits for the current transfer to complete if the receive FIFO is empty.
+ *
+ * <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
+ *
+ * @param initiate If true, this function pushes "0" into the transmit buffer and initiates
+ * a transfer. If false, this function assumes that data is already in the
+ * receive FIFO from a previous write.
+ * @param dataReceived The buffer to be filled with the received data.
+ * @param size The length of the transaction, in bytes
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public int read(boolean initiate, ByteBuffer dataReceived, int size) {
+ if (dataReceived.hasArray()) {
+ return read(initiate, dataReceived.array(), size);
+ }
+ if (!dataReceived.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (dataReceived.capacity() < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiRead(m_port, initiate, dataReceived, (byte) size);
+ }
+
+ /**
+ * Perform a simultaneous read/write transaction with the device.
+ *
+ * @param dataToSend The data to be written out to the device
+ * @param dataReceived Buffer to receive data from the device
+ * @param size The length of the transaction, in bytes
+ */
+ public int transaction(byte[] dataToSend, byte[] dataReceived, int size) {
+ if (dataToSend.length < size) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
+ }
+ if (dataReceived.length < size) {
+ throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
+ }
+ return SPIJNI.spiTransactionB(m_port, dataToSend, dataReceived, (byte) size);
+ }
+
+ /**
+ * Perform a simultaneous read/write transaction with the device.
+ *
+ * @param dataToSend The data to be written out to the device.
+ * @param dataReceived Buffer to receive data from the device.
+ * @param size The length of the transaction, in bytes
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "ByteBufferBackingArray"})
+ public int transaction(ByteBuffer dataToSend, ByteBuffer dataReceived, int size) {
+ if (dataToSend.hasArray() && dataReceived.hasArray()) {
+ return transaction(dataToSend.array(), dataReceived.array(), size);
+ }
+ if (!dataToSend.isDirect()) {
+ throw new IllegalArgumentException("dataToSend must be a direct buffer");
+ }
+ if (dataToSend.capacity() < size) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
+ }
+ if (!dataReceived.isDirect()) {
+ throw new IllegalArgumentException("dataReceived must be a direct buffer");
+ }
+ if (dataReceived.capacity() < size) {
+ throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
+ }
+ return SPIJNI.spiTransaction(m_port, dataToSend, dataReceived, (byte) size);
+ }
+
+ /**
+ * Initialize automatic SPI transfer engine.
+ *
+ * <p>Only a single engine is available, and use of it blocks use of all other
+ * chip select usage on the same physical SPI port while it is running.
+ *
+ * @param bufferSize buffer size in bytes
+ */
+ public void initAuto(int bufferSize) {
+ SPIJNI.spiInitAuto(m_port, bufferSize);
+ }
+
+ /**
+ * Frees the automatic SPI transfer engine.
+ */
+ public void freeAuto() {
+ SPIJNI.spiFreeAuto(m_port);
+ }
+
+ /**
+ * Set the data to be transmitted by the engine.
+ *
+ * <p>Up to 16 bytes are configurable, and may be followed by up to 127 zero
+ * bytes.
+ *
+ * @param dataToSend data to send (maximum 16 bytes)
+ * @param zeroSize number of zeros to send after the data
+ */
+ public void setAutoTransmitData(byte[] dataToSend, int zeroSize) {
+ SPIJNI.spiSetAutoTransmitData(m_port, dataToSend, zeroSize);
+ }
+
+ /**
+ * Start running the automatic SPI transfer engine at a periodic rate.
+ *
+ * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must
+ * be called before calling this function.
+ *
+ * @param period period between transfers, in seconds (us resolution)
+ */
+ public void startAutoRate(double period) {
+ SPIJNI.spiStartAutoRate(m_port, period);
+ }
+
+ /**
+ * Start running the automatic SPI transfer engine when a trigger occurs.
+ *
+ * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must
+ * be called before calling this function.
+ *
+ * @param source digital source for the trigger (may be an analog trigger)
+ * @param rising trigger on the rising edge
+ * @param falling trigger on the falling edge
+ */
+ public void startAutoTrigger(DigitalSource source, boolean rising, boolean falling) {
+ SPIJNI.spiStartAutoTrigger(m_port, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting(), rising, falling);
+ }
+
+ /**
+ * Stop running the automatic SPI transfer engine.
+ */
+ public void stopAuto() {
+ SPIJNI.spiStopAuto(m_port);
+ }
+
+ /**
+ * Force the engine to make a single transfer.
+ */
+ public void forceAutoRead() {
+ SPIJNI.spiForceAutoRead(m_port);
+ }
+
+ /**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * <p>Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * <p>Each received data sequence consists of a timestamp followed by the
+ * received data bytes, one byte per word (in the least significant byte).
+ * The length of each received data sequence is the same as the combined
+ * size of the data and zeroSize set in setAutoTransmitData().
+ *
+ * <p>Blocks until numToRead words have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many words are available.
+ *
+ * @param buffer buffer where read words are stored
+ * @param numToRead number of words to read
+ * @param timeout timeout in seconds (ms resolution)
+ * @return Number of words remaining to be read
+ */
+ public int readAutoReceivedData(ByteBuffer buffer, int numToRead, double timeout) {
+ if (!buffer.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (buffer.capacity() < numToRead * 4) {
+ throw new IllegalArgumentException("buffer is too small, must be at least "
+ + (numToRead * 4));
+ }
+ return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
+ }
+
+ /**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * <p>Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * <p>Each received data sequence consists of a timestamp followed by the
+ * received data bytes, one byte per word (in the least significant byte).
+ * The length of each received data sequence is the same as the combined
+ * size of the data and zeroSize set in setAutoTransmitData().
+ *
+ * <p>Blocks until numToRead words have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many words are available.
+ *
+ * @param buffer array where read words are stored
+ * @param numToRead number of words to read
+ * @param timeout timeout in seconds (ms resolution)
+ * @return Number of words remaining to be read
+ */
+ public int readAutoReceivedData(int[] buffer, int numToRead, double timeout) {
+ if (buffer.length < numToRead) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead);
+ }
+ return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
+ }
+
+ /**
+ * Get the number of bytes dropped by the automatic SPI transfer engine due
+ * to the receive buffer being full.
+ *
+ * @return Number of bytes dropped
+ */
+ public int getAutoDroppedCount() {
+ return SPIJNI.spiGetAutoDroppedCount(m_port);
+ }
+
+ /**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+ public void configureAutoStall(int csToSclkTicks, int stallTicks, int pow2BytesPerRead) {
+ SPIJNI.spiConfigureAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead);
+ }
+
+ private static final int kAccumulateDepth = 2048;
+
+ @SuppressWarnings("PMD.TooManyFields")
+ private static class Accumulator implements AutoCloseable {
+ Accumulator(int port, int xferSize, int validMask, int validValue, int dataShift,
+ int dataSize, boolean isSigned, boolean bigEndian) {
+ m_notifier = new Notifier(this::update);
+ m_buf = ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4)
+ .order(ByteOrder.nativeOrder());
+ m_intBuf = m_buf.asIntBuffer();
+ m_xferSize = xferSize + 1; // +1 for timestamp
+ m_validMask = validMask;
+ m_validValue = validValue;
+ m_dataShift = dataShift;
+ m_dataMax = 1 << dataSize;
+ m_dataMsbMask = 1 << (dataSize - 1);
+ m_isSigned = isSigned;
+ m_bigEndian = bigEndian;
+ m_port = port;
+ }
+
+ @Override
+ public void close() {
+ m_notifier.close();
+ }
+
+ final Notifier m_notifier;
+ final ByteBuffer m_buf;
+ final IntBuffer m_intBuf;
+ final Object m_mutex = new Object();
+
+ long m_value;
+ int m_count;
+ int m_lastValue;
+ long m_lastTimestamp;
+ double m_integratedValue;
+
+ int m_center;
+ int m_deadband;
+ double m_integratedCenter;
+
+ final int m_validMask;
+ final int m_validValue;
+ final int m_dataMax; // one more than max data value
+ final int m_dataMsbMask; // data field MSB mask (for signed)
+ final int m_dataShift; // data field shift right amount, in bits
+ final int m_xferSize; // SPI transfer size, in bytes
+ final boolean m_isSigned; // is data field signed?
+ final boolean m_bigEndian; // is response big endian?
+ final int m_port;
+
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ void update() {
+ synchronized (m_mutex) {
+ boolean done = false;
+ while (!done) {
+ done = true;
+
+ // get amount of data available
+ int numToRead = SPIJNI.spiReadAutoReceivedData(m_port, m_buf, 0, 0);
+
+ // only get whole responses
+ numToRead -= numToRead % m_xferSize;
+ if (numToRead > m_xferSize * kAccumulateDepth) {
+ numToRead = m_xferSize * kAccumulateDepth;
+ done = false;
+ }
+ if (numToRead == 0) {
+ return; // no samples
+ }
+
+ // read buffered data
+ SPIJNI.spiReadAutoReceivedData(m_port, m_buf, numToRead, 0);
+
+ // loop over all responses
+ for (int off = 0; off < numToRead; off += m_xferSize) {
+ // get timestamp from first word
+ long timestamp = m_intBuf.get(off) & 0xffffffffL;
+
+ // convert from bytes
+ int resp = 0;
+ if (m_bigEndian) {
+ for (int i = 1; i < m_xferSize; ++i) {
+ resp <<= 8;
+ resp |= m_intBuf.get(off + i) & 0xff;
+ }
+ } else {
+ for (int i = m_xferSize - 1; i >= 1; --i) {
+ resp <<= 8;
+ resp |= m_intBuf.get(off + i) & 0xff;
+ }
+ }
+
+ // process response
+ if ((resp & m_validMask) == m_validValue) {
+ // valid sensor data; extract data field
+ int data = resp >> m_dataShift;
+ data &= m_dataMax - 1;
+ // 2s complement conversion if signed MSB is set
+ if (m_isSigned && (data & m_dataMsbMask) != 0) {
+ data -= m_dataMax;
+ }
+ // center offset
+ int dataNoCenter = data;
+ data -= m_center;
+ // only accumulate if outside deadband
+ if (data < -m_deadband || data > m_deadband) {
+ m_value += data;
+ if (m_count != 0) {
+ // timestamps use the 1us FPGA clock; also handle rollover
+ if (timestamp >= m_lastTimestamp) {
+ m_integratedValue += dataNoCenter * (timestamp - m_lastTimestamp)
+ * 1e-6 - m_integratedCenter;
+ } else {
+ m_integratedValue += dataNoCenter * ((1L << 32) - m_lastTimestamp + timestamp)
+ * 1e-6 - m_integratedCenter;
+ }
+ }
+ }
+ ++m_count;
+ m_lastValue = data;
+ } else {
+ // no data from the sensor; just clear the last value
+ m_lastValue = 0;
+ }
+ m_lastTimestamp = timestamp;
+ }
+ }
+ }
+ }
+ }
+
+ private Accumulator m_accum;
+
+ /**
+ * Initialize the accumulator.
+ *
+ * @param period Time between reads
+ * @param cmd SPI command to send to request data
+ * @param xferSize SPI transfer size, in bytes
+ * @param validMask Mask to apply to received data for validity checking
+ * @param validValue After validMask is applied, required matching value for validity checking
+ * @param dataShift Bit shift to apply to received data to get actual data value
+ * @param dataSize Size (in bits) of data field
+ * @param isSigned Is data field signed?
+ * @param bigEndian Is device big endian?
+ */
+ public void initAccumulator(double period, int cmd, int xferSize,
+ int validMask, int validValue,
+ int dataShift, int dataSize,
+ boolean isSigned, boolean bigEndian) {
+ initAuto(xferSize * 2048);
+ byte[] cmdBytes = new byte[] {0, 0, 0, 0};
+ if (bigEndian) {
+ for (int i = xferSize - 1; i >= 0; --i) {
+ cmdBytes[i] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ }
+ } else {
+ cmdBytes[0] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ cmdBytes[1] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ cmdBytes[2] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ cmdBytes[3] = (byte) (cmd & 0xff);
+ }
+ setAutoTransmitData(cmdBytes, xferSize - 4);
+ startAutoRate(period);
+
+ m_accum = new Accumulator(m_port, xferSize, validMask, validValue, dataShift, dataSize,
+ isSigned, bigEndian);
+ m_accum.m_notifier.startPeriodic(period * 1024);
+ }
+
+ /**
+ * Frees the accumulator.
+ */
+ public void freeAccumulator() {
+ if (m_accum != null) {
+ m_accum.close();
+ m_accum = null;
+ }
+ freeAuto();
+ }
+
+ /**
+ * Resets the accumulator to zero.
+ */
+ public void resetAccumulator() {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_value = 0;
+ m_accum.m_count = 0;
+ m_accum.m_lastValue = 0;
+ m_accum.m_lastTimestamp = 0;
+ m_accum.m_integratedValue = 0;
+ }
+ }
+
+ /**
+ * Set the center value of the accumulator.
+ *
+ * <p>The center value is subtracted from each value before it is added to the accumulator. This
+ * is used for the center value of devices like gyros and accelerometers to make integration work
+ * and to take the device offset into account when integrating.
+ */
+ public void setAccumulatorCenter(int center) {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_center = center;
+ }
+ }
+
+ /**
+ * Set the accumulator's deadband.
+ */
+ public void setAccumulatorDeadband(int deadband) {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_deadband = deadband;
+ }
+ }
+
+ /**
+ * Read the last value read by the accumulator engine.
+ */
+ public int getAccumulatorLastValue() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_lastValue;
+ }
+ }
+
+ /**
+ * Read the accumulated value.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+ public long getAccumulatorValue() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_value;
+ }
+ }
+
+ /**
+ * Read the number of accumulated values.
+ *
+ * <p>Read the count of the accumulated values since the accumulator was last Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+ public int getAccumulatorCount() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_count;
+ }
+ }
+
+ /**
+ * Read the average of the accumulated value.
+ *
+ * @return The accumulated average value (value / count).
+ */
+ public double getAccumulatorAverage() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ if (m_accum.m_count == 0) {
+ return 0.0;
+ }
+ return ((double) m_accum.m_value) / m_accum.m_count;
+ }
+ }
+
+ /**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * <p>This function reads the value and count atomically. This can be used for averaging.
+ *
+ * @param result AccumulatorResult object to store the results in.
+ */
+ public void getAccumulatorOutput(AccumulatorResult result) {
+ if (result == null) {
+ throw new IllegalArgumentException("Null parameter `result'");
+ }
+ if (m_accum == null) {
+ result.value = 0;
+ result.count = 0;
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ result.value = m_accum.m_value;
+ result.count = m_accum.m_count;
+ }
+ }
+
+ /**
+ * Set the center value of the accumulator integrator.
+ *
+ * <p>The center value is subtracted from each value*dt before it is added to the
+ * integrated value. This is used for the center value of devices like gyros
+ * and accelerometers to take the device offset into account when integrating.
+ */
+ public void setAccumulatorIntegratedCenter(double center) {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_integratedCenter = center;
+ }
+ }
+
+ /**
+ * Read the integrated value. This is the sum of (each value * time between
+ * values).
+ *
+ * @return The integrated value accumulated since the last Reset().
+ */
+ public double getAccumulatorIntegratedValue() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_integratedValue;
+ }
+ }
+
+ /**
+ * Read the average of the integrated value. This is the sum of (each value
+ * times the time between values), divided by the count.
+ *
+ * @return The average of the integrated value accumulated since the last
+ * Reset().
+ */
+ public double getAccumulatorIntegratedAverage() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ if (m_accum.m_count <= 1) {
+ return 0.0;
+ }
+ // count-1 due to not integrating the first value received
+ return m_accum.m_integratedValue / (m_accum.m_count - 1);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
new file mode 100644
index 0000000..138f7f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+
+/**
+ * The base interface for objects that can be sent over the network through network tables.
+ */
+public interface Sendable {
+ /**
+ * Gets the name of this {@link Sendable} object.
+ *
+ * @return Name
+ * @deprecated Use SendableRegistry.getName()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default String getName() {
+ return SendableRegistry.getName(this);
+ }
+
+ /**
+ * Sets the name of this {@link Sendable} object.
+ *
+ * @param name name
+ * @deprecated Use SendableRegistry.setName()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default void setName(String name) {
+ SendableRegistry.setName(this, name);
+ }
+
+ /**
+ * Sets both the subsystem name and device name of this {@link Sendable} object.
+ *
+ * @param subsystem subsystem name
+ * @param name device name
+ * @deprecated Use SendableRegistry.setName()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default void setName(String subsystem, String name) {
+ SendableRegistry.setName(this, subsystem, name);
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ * @deprecated Use SendableRegistry.setName()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default void setName(String moduleType, int channel) {
+ SendableRegistry.setName(this, moduleType, channel);
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ * @deprecated Use SendableRegistry.setName()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default void setName(String moduleType, int moduleNumber, int channel) {
+ SendableRegistry.setName(this, moduleType, moduleNumber, channel);
+ }
+
+ /**
+ * Gets the subsystem name of this {@link Sendable} object.
+ *
+ * @return Subsystem name
+ * @deprecated Use SendableRegistry.getSubsystem()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default String getSubsystem() {
+ return SendableRegistry.getSubsystem(this);
+ }
+
+ /**
+ * Sets the subsystem name of this {@link Sendable} object.
+ *
+ * @param subsystem subsystem name
+ * @deprecated Use SendableRegistry.setSubsystem()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default void setSubsystem(String subsystem) {
+ SendableRegistry.setSubsystem(this, subsystem);
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ * @deprecated Use SendableRegistry.addChild()
+ */
+ @Deprecated(since = "2020", forRemoval = true)
+ default void addChild(Object child) {
+ SendableRegistry.addChild(this, child);
+ }
+
+ /**
+ * Initializes this {@link Sendable} object.
+ *
+ * @param builder sendable builder
+ */
+ void initSendable(SendableBuilder builder);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
new file mode 100644
index 0000000..4e2116c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Base class for all sensors. Stores most recent status information as well as containing utility
+ * functions for checking channels and error processing.
+ * @deprecated Use Sendable and SendableRegistry
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public abstract class SendableBase implements Sendable, AutoCloseable {
+ /**
+ * Creates an instance of the sensor base.
+ */
+ public SendableBase() {
+ this(true);
+ }
+
+ /**
+ * Creates an instance of the sensor base.
+ *
+ * @param addLiveWindow if true, add this Sendable to LiveWindow
+ */
+ public SendableBase(boolean addLiveWindow) {
+ if (addLiveWindow) {
+ SendableRegistry.addLW(this, "");
+ } else {
+ SendableRegistry.add(this, "");
+ }
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
new file mode 100644
index 0000000..4f2b8b5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
@@ -0,0 +1,236 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.ConstantsJNI;
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.PDPJNI;
+import edu.wpi.first.hal.PWMJNI;
+import edu.wpi.first.hal.PortsJNI;
+import edu.wpi.first.hal.RelayJNI;
+import edu.wpi.first.hal.SolenoidJNI;
+
+/**
+ * Stores most recent status information as well as containing utility functions for checking
+ * channels and error processing.
+ */
+public final class SensorUtil {
+ /**
+ * Ticks per microsecond.
+ */
+ public static final int kSystemClockTicksPerMicrosecond
+ = ConstantsJNI.getSystemClockTicksPerMicrosecond();
+
+ /**
+ * Number of digital channels per roboRIO.
+ */
+ public static final int kDigitalChannels = PortsJNI.getNumDigitalChannels();
+
+ /**
+ * Number of analog input channels per roboRIO.
+ */
+ public static final int kAnalogInputChannels = PortsJNI.getNumAnalogInputs();
+
+ /**
+ * Number of analog output channels per roboRIO.
+ */
+ public static final int kAnalogOutputChannels = PortsJNI.getNumAnalogOutputs();
+
+ /**
+ * Number of solenoid channels per module.
+ */
+ public static final int kSolenoidChannels = PortsJNI.getNumSolenoidChannels();
+
+ /**
+ * Number of PWM channels per roboRIO.
+ */
+ public static final int kPwmChannels = PortsJNI.getNumPWMChannels();
+
+ /**
+ * Number of relay channels per roboRIO.
+ */
+ public static final int kRelayChannels = PortsJNI.getNumRelayHeaders();
+
+ /**
+ * Number of power distribution channels per PDP.
+ */
+ public static final int kPDPChannels = PortsJNI.getNumPDPChannels();
+
+ /**
+ * Number of power distribution modules per PDP.
+ */
+ public static final int kPDPModules = PortsJNI.getNumPDPModules();
+
+ /**
+ * Number of PCM Modules.
+ */
+ public static final int kPCMModules = PortsJNI.getNumPCMModules();
+
+ /**
+ * Verify that the solenoid module is correct.
+ *
+ * @param moduleNumber The solenoid module module number to check.
+ */
+ public static void checkSolenoidModule(final int moduleNumber) {
+ if (!SolenoidJNI.checkSolenoidModule(moduleNumber)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested solenoid module is out of range. Minimum: 0, Maximum: ")
+ .append(kPCMModules)
+ .append(", Requested: ")
+ .append(moduleNumber);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the digital channel number is valid. Verify that the channel number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkDigitalChannel(final int channel) {
+ if (!DIOJNI.checkDIOChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested DIO channel is out of range. Minimum: 0, Maximum: ")
+ .append(kDigitalChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the digital channel number is valid. Verify that the channel number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkRelayChannel(final int channel) {
+ if (!RelayJNI.checkRelayChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested relay channel is out of range. Minimum: 0, Maximum: ")
+ .append(kRelayChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the digital channel number is valid. Verify that the channel number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkPWMChannel(final int channel) {
+ if (!PWMJNI.checkPWMChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested PWM channel is out of range. Minimum: 0, Maximum: ")
+ .append(kPwmChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the analog input number is value. Verify that the analog input number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkAnalogInputChannel(final int channel) {
+ if (!AnalogJNI.checkAnalogInputChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested analog input channel is out of range. Minimum: 0, Maximum: ")
+ .append(kAnalogInputChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the analog input number is value. Verify that the analog input number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkAnalogOutputChannel(final int channel) {
+ if (!AnalogJNI.checkAnalogOutputChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested analog output channel is out of range. Minimum: 0, Maximum: ")
+ .append(kAnalogOutputChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Verify that the solenoid channel number is within limits. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkSolenoidChannel(final int channel) {
+ if (!SolenoidJNI.checkSolenoidChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested solenoid channel is out of range. Minimum: 0, Maximum: ")
+ .append(kSolenoidChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Verify that the power distribution channel number is within limits. Channel numbers are
+ * 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkPDPChannel(final int channel) {
+ if (!PDPJNI.checkPDPChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested PDP channel is out of range. Minimum: 0, Maximum: ")
+ .append(kPDPChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Verify that the PDP module number is within limits. module numbers are 0-based.
+ *
+ * @param module The module number to check.
+ */
+ public static void checkPDPModule(final int module) {
+ if (!PDPJNI.checkPDPModule(module)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested PDP module is out of range. Minimum: 0, Maximum: ")
+ .append(kPDPModules)
+ .append(", Requested: ")
+ .append(module);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Get the number of the default solenoid module.
+ *
+ * @return The number of the default solenoid module.
+ */
+ public static int getDefaultSolenoidModule() {
+ return 0;
+ }
+
+ private SensorUtil() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
new file mode 100644
index 0000000..86bd941
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -0,0 +1,378 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.charset.StandardCharsets;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SerialPortJNI;
+
+/**
+ * Driver for the serial ports (USB, MXP, Onboard) on the roboRIO.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class SerialPort implements AutoCloseable {
+ private int m_portHandle;
+
+ public enum Port {
+ kOnboard(0), kMXP(1), kUSB(2), kUSB1(2), kUSB2(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Port(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents the parity to use for serial communications.
+ */
+ public enum Parity {
+ kNone(0), kOdd(1), kEven(2), kMark(3), kSpace(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Parity(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents the number of stop bits to use for Serial Communication.
+ */
+ public enum StopBits {
+ kOne(10), kOnePointFive(15), kTwo(20);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ StopBits(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents what type of flow control to use for serial communication.
+ */
+ public enum FlowControl {
+ kNone(0), kXonXoff(1), kRtsCts(2), kDtsDsr(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ FlowControl(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents which type of buffer mode to use when writing to a serial port.
+ */
+ public enum WriteBufferMode {
+ kFlushOnAccess(1), kFlushWhenFull(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ WriteBufferMode(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Create an instance of a Serial Port class.
+ *
+ * <p>Prefer to use the constructor that doesn't take a port name, but in some
+ * cases the automatic detection might not work correctly.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param port The Serial port to use
+ * @param portName The direct portName to use
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ * @param stopBits The number of stop bits to use as defined by the enum StopBits.
+ * @deprecated Will be removed for 2019
+ */
+ public SerialPort(final int baudRate, String portName, Port port, final int dataBits,
+ Parity parity, StopBits stopBits) {
+ m_portHandle = SerialPortJNI.serialInitializePortDirect((byte) port.value, portName);
+ SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
+ SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
+ SerialPortJNI.serialSetParity(m_portHandle, (byte) parity.value);
+ SerialPortJNI.serialSetStopBits(m_portHandle, (byte) stopBits.value);
+
+ // Set the default read buffer size to 1 to return bytes immediately
+ setReadBufferSize(1);
+
+ // Set the default timeout to 5 seconds.
+ setTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+
+ disableTermination();
+
+ HAL.report(tResourceType.kResourceType_SerialPort, port.value + 1);
+ }
+
+ /**
+ * Create an instance of a Serial Port class.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param port The Serial port to use
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ * @param stopBits The number of stop bits to use as defined by the enum StopBits.
+ */
+ public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity,
+ StopBits stopBits) {
+ m_portHandle = SerialPortJNI.serialInitializePort((byte) port.value);
+ SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
+ SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
+ SerialPortJNI.serialSetParity(m_portHandle, (byte) parity.value);
+ SerialPortJNI.serialSetStopBits(m_portHandle, (byte) stopBits.value);
+
+ // Set the default read buffer size to 1 to return bytes immediately
+ setReadBufferSize(1);
+
+ // Set the default timeout to 5 seconds.
+ setTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+
+ disableTermination();
+
+ HAL.report(tResourceType.kResourceType_SerialPort, port.value + 1);
+ }
+
+ /**
+ * Create an instance of a Serial Port class. Defaults to one stop bit.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ */
+ public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity) {
+ this(baudRate, port, dataBits, parity, StopBits.kOne);
+ }
+
+ /**
+ * Create an instance of a Serial Port class. Defaults to no parity and one stop bit.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ */
+ public SerialPort(final int baudRate, Port port, final int dataBits) {
+ this(baudRate, port, dataBits, Parity.kNone, StopBits.kOne);
+ }
+
+ /**
+ * Create an instance of a Serial Port class. Defaults to 8 databits, no parity, and one stop
+ * bit.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ */
+ public SerialPort(final int baudRate, Port port) {
+ this(baudRate, port, 8, Parity.kNone, StopBits.kOne);
+ }
+
+ @Override
+ public void close() {
+ SerialPortJNI.serialClose(m_portHandle);
+ }
+
+ /**
+ * Set the type of flow control to enable on this port.
+ *
+ * <p>By default, flow control is disabled.
+ *
+ * @param flowControl the FlowControl m_value to use
+ */
+ public void setFlowControl(FlowControl flowControl) {
+ SerialPortJNI.serialSetFlowControl(m_portHandle, (byte) flowControl.value);
+ }
+
+ /**
+ * Enable termination and specify the termination character.
+ *
+ * <p>Termination is currently only implemented for receive. When the the terminator is received,
+ * the read() or readString() will return fewer bytes than requested, stopping after the
+ * terminator.
+ *
+ * @param terminator The character to use for termination.
+ */
+ public void enableTermination(char terminator) {
+ SerialPortJNI.serialEnableTermination(m_portHandle, terminator);
+ }
+
+ /**
+ * Enable termination with the default terminator '\n'
+ *
+ * <p>Termination is currently only implemented for receive. When the the terminator is received,
+ * the read() or readString() will return fewer bytes than requested, stopping after the
+ * terminator.
+ *
+ * <p>The default terminator is '\n'
+ */
+ public void enableTermination() {
+ enableTermination('\n');
+ }
+
+ /**
+ * Disable termination behavior.
+ */
+ public void disableTermination() {
+ SerialPortJNI.serialDisableTermination(m_portHandle);
+ }
+
+ /**
+ * Get the number of bytes currently available to read from the serial port.
+ *
+ * @return The number of bytes available to read.
+ */
+ public int getBytesReceived() {
+ return SerialPortJNI.serialGetBytesReceived(m_portHandle);
+ }
+
+ /**
+ * Read a string out of the buffer. Reads the entire contents of the buffer
+ *
+ * @return The read string
+ */
+ public String readString() {
+ return readString(getBytesReceived());
+ }
+
+ /**
+ * Read a string out of the buffer. Reads the entire contents of the buffer
+ *
+ * @param count the number of characters to read into the string
+ * @return The read string
+ */
+ public String readString(int count) {
+ byte[] out = read(count);
+ return new String(out, 0, out.length, StandardCharsets.US_ASCII);
+ }
+
+ /**
+ * Read raw bytes out of the buffer.
+ *
+ * @param count The maximum number of bytes to read.
+ * @return An array of the read bytes
+ */
+ public byte[] read(final int count) {
+ byte[] dataReceivedBuffer = new byte[count];
+ int gotten = SerialPortJNI.serialRead(m_portHandle, dataReceivedBuffer, count);
+ if (gotten == count) {
+ return dataReceivedBuffer;
+ }
+ byte[] retVal = new byte[gotten];
+ System.arraycopy(dataReceivedBuffer, 0, retVal, 0, gotten);
+ return retVal;
+ }
+
+ /**
+ * Write raw bytes to the serial port.
+ *
+ * @param buffer The buffer of bytes to write.
+ * @param count The maximum number of bytes to write.
+ * @return The number of bytes actually written into the port.
+ */
+ public int write(byte[] buffer, int count) {
+ if (buffer.length < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+ return SerialPortJNI.serialWrite(m_portHandle, buffer, count);
+ }
+
+ /**
+ * Write a string to the serial port.
+ *
+ * @param data The string to write to the serial port.
+ * @return The number of bytes actually written into the port.
+ */
+ public int writeString(String data) {
+ return write(data.getBytes(StandardCharsets.UTF_8), data.length());
+ }
+
+ /**
+ * Configure the timeout of the serial m_port.
+ *
+ * <p>This defines the timeout for transactions with the hardware. It will affect reads if less
+ * bytes are available than the read buffer size (defaults to 1) and very large writes.
+ *
+ * @param timeout The number of seconds to to wait for I/O.
+ */
+ public void setTimeout(double timeout) {
+ SerialPortJNI.serialSetTimeout(m_portHandle, timeout);
+ }
+
+ /**
+ * Specify the size of the input buffer.
+ *
+ * <p>Specify the amount of data that can be stored before data from the device is returned to
+ * Read. If you want data that is received to be returned immediately, set this to 1.
+ *
+ * <p>It the buffer is not filled before the read timeout expires, all data that has been received
+ * so far will be returned.
+ *
+ * @param size The read buffer size.
+ */
+ public void setReadBufferSize(int size) {
+ SerialPortJNI.serialSetReadBufferSize(m_portHandle, size);
+ }
+
+ /**
+ * Specify the size of the output buffer.
+ *
+ * <p>Specify the amount of data that can be stored before being transmitted to the device.
+ *
+ * @param size The write buffer size.
+ */
+ public void setWriteBufferSize(int size) {
+ SerialPortJNI.serialSetWriteBufferSize(m_portHandle, size);
+ }
+
+ /**
+ * Specify the flushing behavior of the output buffer.
+ *
+ * <p>When set to kFlushOnAccess, data is synchronously written to the serial port after each
+ * call to either print() or write().
+ *
+ * <p>When set to kFlushWhenFull, data will only be written to the serial port when the buffer
+ * is full or when flush() is called.
+ *
+ * @param mode The write buffer mode.
+ */
+ public void setWriteBufferMode(WriteBufferMode mode) {
+ SerialPortJNI.serialSetWriteMode(m_portHandle, (byte) mode.value);
+ }
+
+ /**
+ * Force the output buffer to be written to the port.
+ *
+ * <p>This is used when setWriteBufferMode() is set to kFlushWhenFull to force a flush before the
+ * buffer is full.
+ */
+ public void flush() {
+ SerialPortJNI.serialFlush(m_portHandle);
+ }
+
+ /**
+ * Reset the serial port driver to a known state.
+ *
+ * <p>Empty the transmit and receive buffers in the device and formatted I/O.
+ */
+ public void reset() {
+ SerialPortJNI.serialClear(m_portHandle);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
new file mode 100644
index 0000000..6b2eab6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Standard hobby style servo.
+ *
+ * <p>The range parameters default to the appropriate values for the Hitec HS-322HD servo provided
+ * in the FIRST Kit of Parts in 2008.
+ */
+public class Servo extends PWM {
+ private static final double kMaxServoAngle = 180.0;
+ private static final double kMinServoAngle = 0.0;
+
+ protected static final double kDefaultMaxServoPWM = 2.4;
+ protected static final double kDefaultMinServoPWM = 0.6;
+
+ /**
+ * Constructor.<br>
+ *
+ * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br> By default
+ * {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
+ *
+ * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Servo(final int channel) {
+ super(channel);
+ setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
+ setPeriodMultiplier(PeriodMultiplier.k4X);
+
+ HAL.report(tResourceType.kResourceType_Servo, getChannel() + 1);
+ SendableRegistry.setName(this, "Servo", getChannel());
+ }
+
+
+ /**
+ * Set the servo position.
+ *
+ * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
+ *
+ * @param value Position from 0.0 to 1.0.
+ */
+ public void set(double value) {
+ setPosition(value);
+ }
+
+ /**
+ * Get the servo position.
+ *
+ * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
+ *
+ * @return Position from 0.0 to 1.0.
+ */
+ public double get() {
+ return getPosition();
+ }
+
+ /**
+ * Set the servo angle.
+ *
+ * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
+ * test).
+ *
+ * <p>Servo angles that are out of the supported range of the servo simply "saturate" in that
+ * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of
+ * less than X result in an angle of X being set and angles of more than Y degrees result in an
+ * angle of Y being set.
+ *
+ * @param degrees The angle in degrees to set the servo.
+ */
+ public void setAngle(double degrees) {
+ if (degrees < kMinServoAngle) {
+ degrees = kMinServoAngle;
+ } else if (degrees > kMaxServoAngle) {
+ degrees = kMaxServoAngle;
+ }
+
+ setPosition(((degrees - kMinServoAngle)) / getServoAngleRange());
+ }
+
+ /**
+ * Get the servo angle.
+ *
+ * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
+ * test).
+ *
+ * @return The angle in degrees to which the servo is set.
+ */
+ public double getAngle() {
+ return getPosition() * getServoAngleRange() + kMinServoAngle;
+ }
+
+ private double getServoAngleRange() {
+ return kMaxServoAngle - kMinServoAngle;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Servo");
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
new file mode 100644
index 0000000..2ba468b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a
+ * {@link edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
+ */
+public class SlewRateLimiter {
+ private final Timer m_timer = new Timer();
+ private final double m_rateLimit;
+ private double m_prevVal;
+
+ /**
+ * Creates a new SlewRateLimiter with the given rate limit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ * @param initialValue The initial value of the input.
+ */
+ public SlewRateLimiter(double rateLimit, double initialValue) {
+ m_prevVal = initialValue;
+ m_rateLimit = rateLimit;
+ m_timer.start();
+ }
+
+ /**
+ * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ */
+ public SlewRateLimiter(double rateLimit) {
+ this(rateLimit, 0);
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input) {
+ m_prevVal += MathUtil.clamp(input - m_prevVal,
+ -m_rateLimit * m_timer.get(),
+ m_rateLimit * m_timer.get());
+ m_timer.reset();
+ return m_prevVal;
+ }
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ public void reset(double value) {
+ m_timer.reset();
+ m_prevVal = value;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
new file mode 100644
index 0000000..163415c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SolenoidJNI;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Solenoid class for running high voltage Digital Output on the PCM.
+ *
+ * <p>The Solenoid class is typically used for pneumatic solenoids, but could be used for any
+ * device within the current spec of the PCM.
+ */
+public class Solenoid extends SolenoidBase implements Sendable, AutoCloseable {
+ private final int m_channel; // The channel to control.
+ private int m_solenoidHandle;
+
+ /**
+ * Constructor using the default PCM ID (defaults to 0).
+ *
+ * @param channel The channel on the PCM to control (0..7).
+ */
+ public Solenoid(final int channel) {
+ this(SensorUtil.getDefaultSolenoidModule(), channel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The CAN ID of the PCM the solenoid is attached to.
+ * @param channel The channel on the PCM to control (0..7).
+ */
+ public Solenoid(final int moduleNumber, final int channel) {
+ super(moduleNumber);
+ m_channel = channel;
+
+ SensorUtil.checkSolenoidModule(m_moduleNumber);
+ SensorUtil.checkSolenoidChannel(m_channel);
+
+ int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) m_channel);
+ m_solenoidHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+
+ HAL.report(tResourceType.kResourceType_Solenoid, m_channel + 1, m_moduleNumber + 1);
+ SendableRegistry.addLW(this, "Solenoid", m_moduleNumber, m_channel);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ SolenoidJNI.freeSolenoidPort(m_solenoidHandle);
+ m_solenoidHandle = 0;
+ }
+
+ /**
+ * Set the value of a solenoid.
+ *
+ * @param on True will turn the solenoid output on. False will turn the solenoid output off.
+ */
+ public void set(boolean on) {
+ SolenoidJNI.setSolenoid(m_solenoidHandle, on);
+ }
+
+ /**
+ * Read the current value of the solenoid.
+ *
+ * @return True if the solenoid output is on or false if the solenoid output is off.
+ */
+ public boolean get() {
+ return SolenoidJNI.getSolenoid(m_solenoidHandle);
+ }
+
+ /**
+ * Check if solenoid is blacklisted. If a solenoid is shorted, it is added to the blacklist and
+ * disabled until power cycle, or until faults are cleared.
+ *
+ * @return If solenoid is disabled due to short.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public boolean isBlackListed() {
+ int value = getPCMSolenoidBlackList() & (1 << m_channel);
+ return value != 0;
+ }
+
+ /**
+ * Set the pulse duration in the PCM. This is used in conjunction with
+ * the startPulse method to allow the PCM to control the timing of a pulse.
+ * The timing can be controlled in 0.01 second increments.
+ *
+ * @param durationSeconds The duration of the pulse, from 0.01 to 2.55 seconds.
+ *
+ * @see #startPulse()
+ */
+ public void setPulseDuration(double durationSeconds) {
+ long durationMS = (long) (durationSeconds * 1000);
+ SolenoidJNI.setOneShotDuration(m_solenoidHandle, durationMS);
+ }
+
+ /**
+ * Trigger the PCM to generate a pulse of the duration set in
+ * setPulseDuration.
+ *
+ * @see #setPulseDuration(double)
+ */
+ public void startPulse() {
+ SolenoidJNI.fireOneShot(m_solenoidHandle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Solenoid");
+ builder.setActuator(true);
+ builder.setSafeState(() -> set(false));
+ builder.addBooleanProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
new file mode 100644
index 0000000..a778d8c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.SolenoidJNI;
+
+/**
+ * SolenoidBase class is the common base class for the {@link Solenoid} and {@link DoubleSolenoid}
+ * classes.
+ */
+public class SolenoidBase {
+ protected final int m_moduleNumber; // The number of the solenoid module being used.
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The PCM CAN ID
+ */
+ protected SolenoidBase(final int moduleNumber) {
+ m_moduleNumber = moduleNumber;
+ }
+
+ /**
+ * Read all 8 solenoids from the specified module as a single byte.
+ *
+ * @param moduleNumber the module number to read
+ * @return The current value of all 8 solenoids on the module.
+ */
+ public static int getAll(int moduleNumber) {
+ return SolenoidJNI.getAllSolenoids(moduleNumber);
+ }
+
+ /**
+ * Read all 8 solenoids from the module used by this solenoid as a single byte.
+ *
+ * @return The current value of all 8 solenoids on this module.
+ */
+ public int getAll() {
+ return SolenoidBase.getAll(m_moduleNumber);
+ }
+
+ /**
+ * Reads complete solenoid blacklist for all 8 solenoids as a single byte. If a solenoid is
+ * shorted, it is added to the blacklist and disabled until power cycle, or until faults are
+ * cleared.
+ *
+ * @param moduleNumber the module number to read
+ * @return The solenoid blacklist of all 8 solenoids on the module.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public static int getPCMSolenoidBlackList(int moduleNumber) {
+ return SolenoidJNI.getPCMSolenoidBlackList(moduleNumber);
+ }
+
+ /**
+ * Reads complete solenoid blacklist for all 8 solenoids as a single byte. If a solenoid is
+ * shorted, it is added to the blacklist and disabled until power cycle, or until faults are
+ * cleared.
+ *
+ * @return The solenoid blacklist of all 8 solenoids on the module.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public int getPCMSolenoidBlackList() {
+ return SolenoidBase.getPCMSolenoidBlackList(m_moduleNumber);
+ }
+
+ /**
+ * If true, the common highside solenoid voltage rail is too low, most likely a solenoid channel
+ * is shorted.
+ *
+ * @param moduleNumber the module number to read
+ * @return true if PCM sticky fault is set
+ */
+ public static boolean getPCMSolenoidVoltageStickyFault(int moduleNumber) {
+ return SolenoidJNI.getPCMSolenoidVoltageStickyFault(moduleNumber);
+ }
+
+ /**
+ * If true, the common highside solenoid voltage rail is too low, most likely a solenoid channel
+ * is shorted.
+ *
+ * @return true if PCM sticky fault is set
+ */
+ public boolean getPCMSolenoidVoltageStickyFault() {
+ return SolenoidBase.getPCMSolenoidVoltageStickyFault(m_moduleNumber);
+ }
+
+ /**
+ * The common highside solenoid voltage rail is too low, most likely a solenoid channel is
+ * shorted.
+ *
+ * @param moduleNumber the module number to read
+ * @return true if PCM is in fault state.
+ */
+ public static boolean getPCMSolenoidVoltageFault(int moduleNumber) {
+ return SolenoidJNI.getPCMSolenoidVoltageFault(moduleNumber);
+ }
+
+ /**
+ * The common highside solenoid voltage rail is too low, most likely a solenoid channel is
+ * shorted.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getPCMSolenoidVoltageFault() {
+ return SolenoidBase.getPCMSolenoidVoltageFault(m_moduleNumber);
+ }
+
+ /**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * <p>If a sticky fault is set, then it will be persistently cleared. Compressor drive maybe
+ * momentarily disable while flags are being cleared. Care should be taken to not call this too
+ * frequently, otherwise normal compressor functionality may be prevented.
+ *
+ * <p>If no sticky faults are set then this call will have no effect.
+ *
+ * @param moduleNumber the module number to read
+ */
+ public static void clearAllPCMStickyFaults(int moduleNumber) {
+ SolenoidJNI.clearAllPCMStickyFaults(moduleNumber);
+ }
+
+ /**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * <p>If a sticky fault is set, then it will be persistently cleared. Compressor drive maybe
+ * momentarily disable while flags are being cleared. Care should be taken to not call this too
+ * frequently, otherwise normal compressor functionality may be prevented.
+ *
+ * <p>If no sticky faults are set then this call will have no effect.
+ */
+ public void clearAllPCMStickyFaults() {
+ SolenoidBase.clearAllPCMStickyFaults(m_moduleNumber);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
new file mode 100644
index 0000000..c8bacbf
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * REV Robotics SPARK Speed Controller.
+ *
+ * <p>Note that the SPARK uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the Spark User Manual
+ * available from REV Robotics.
+ *
+ * <p><ul>
+ * <li>2.003ms = full "forward"
+ * <li>1.550ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.460ms = the "low end" of the deadband range
+ * <li>0.999ms = full "reverse"
+ * </ul>
+ */
+public class Spark extends PWMSpeedController {
+ /**
+ * Common initialization code called by all constructors.
+ */
+ protected void initSpark() {
+ setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
+ SendableRegistry.setName(this, "Spark", getChannel());
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the SPARK is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Spark(final int channel) {
+ super(channel);
+ initSpark();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
new file mode 100644
index 0000000..4407ff0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for speed controlling devices.
+ */
+public interface SpeedController extends PIDOutput {
+ /**
+ * Common interface for setting the speed of a speed controller.
+ *
+ * @param speed The speed to set. Value should be between -1.0 and 1.0.
+ */
+ void set(double speed);
+
+ /**
+ * Sets the voltage output of the SpeedController. Compensates for the current bus
+ * voltage to ensure that the desired voltage is output even if the battery voltage is below
+ * 12V - highly useful when the voltage outputs are "meaningful" (e.g. they come from a
+ * feedforward calculation).
+ *
+ * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
+ * properly - unlike the ordinary set function, it is not "set it and forget it."
+ *
+ * @param outputVolts The voltage to output.
+ */
+ default void setVoltage(double outputVolts) {
+ set(outputVolts / RobotController.getBatteryVoltage());
+ }
+
+ /**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+ double get();
+
+ /**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion true is inverted.
+ */
+ void setInverted(boolean isInverted);
+
+ /**
+ * Common interface for returning if a speed controller is in the inverted state or not.
+ *
+ * @return isInverted The state of the inversion true is inverted.
+ */
+ boolean getInverted();
+
+ /**
+ * Disable the speed controller.
+ */
+ void disable();
+
+ /**
+ * Stops motor movement. Motor can be moved again by calling set without having to re-enable the
+ * motor.
+ */
+ void stopMotor();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
new file mode 100644
index 0000000..7dd3f76
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Allows multiple {@link SpeedController} objects to be linked together.
+ */
+public class SpeedControllerGroup implements SpeedController, Sendable, AutoCloseable {
+ private boolean m_isInverted;
+ private final SpeedController[] m_speedControllers;
+ private static int instances;
+
+ /**
+ * Create a new SpeedControllerGroup with the provided SpeedControllers.
+ *
+ * @param speedControllers The SpeedControllers to add
+ */
+ @SuppressWarnings("PMD.AvoidArrayLoops")
+ public SpeedControllerGroup(SpeedController speedController,
+ SpeedController... speedControllers) {
+ m_speedControllers = new SpeedController[speedControllers.length + 1];
+ m_speedControllers[0] = speedController;
+ SendableRegistry.addChild(this, speedController);
+ for (int i = 0; i < speedControllers.length; i++) {
+ m_speedControllers[i + 1] = speedControllers[i];
+ SendableRegistry.addChild(this, speedControllers[i]);
+ }
+ instances++;
+ SendableRegistry.addLW(this, "tSpeedControllerGroup", instances);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ @Override
+ public void set(double speed) {
+ for (SpeedController speedController : m_speedControllers) {
+ speedController.set(m_isInverted ? -speed : speed);
+ }
+ }
+
+ @Override
+ public double get() {
+ if (m_speedControllers.length > 0) {
+ return m_speedControllers[0].get() * (m_isInverted ? -1 : 1);
+ }
+ return 0.0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ @Override
+ public void disable() {
+ for (SpeedController speedController : m_speedControllers) {
+ speedController.disable();
+ }
+ }
+
+ @Override
+ public void stopMotor() {
+ for (SpeedController speedController : m_speedControllers) {
+ speedController.stopMotor();
+ }
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Speed Controller");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
new file mode 100644
index 0000000..6f20869
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
+ *
+ * <p>Note that the Talon uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the Talon User Manual
+ * available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.037ms = full "forward"
+ * <li>1.539ms = the "high end" of the deadband range
+ * <li>1.513ms = center of the deadband range (off)
+ * <li>1.487ms = the "low end" of the deadband range
+ * <li>0.989ms = full "reverse"
+ * </ul>
+ */
+public class Talon extends PWMSpeedController {
+ /**
+ * Constructor for a Talon (original or Talon SR).
+ *
+ * @param channel The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Talon(final int channel) {
+ super(channel);
+
+ setBounds(2.037, 1.539, 1.513, 1.487, 0.989);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_Talon, getChannel() + 1);
+ SendableRegistry.setName(this, "Talon", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
new file mode 100644
index 0000000..7b49d4d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.ThreadsJNI;
+
+public final class Threads {
+ /**
+ * Get the thread priority for the current thread.
+ * @return The current thread priority. Scaled 1-99.
+ */
+ public static int getCurrentThreadPriority() {
+ return ThreadsJNI.getCurrentThreadPriority();
+ }
+
+ /**
+ * Get if the current thread is realtime.
+ * @return If the current thread is realtime
+ */
+ public static boolean getCurrentThreadIsRealTime() {
+ return ThreadsJNI.getCurrentThreadIsRealTime();
+ }
+
+ /**
+ * Sets the thread priority for the current thread.
+ *
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest. On RoboRIO, priority is ignored for non realtime setting
+ *
+ * @return The success state of setting the priority
+ */
+ public static boolean setCurrentThreadPriority(boolean realTime, int priority) {
+ return ThreadsJNI.setCurrentThreadPriority(realTime, priority);
+ }
+
+ private Threads() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
new file mode 100644
index 0000000..b6b6b25
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.NotifierJNI;
+
+/**
+ * TimedRobot implements the IterativeRobotBase robot program framework.
+ *
+ * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
+ */
+public class TimedRobot extends IterativeRobotBase {
+ public static final double kDefaultPeriod = 0.02;
+
+ // The C pointer to the notifier object. We don't use it directly, it is
+ // just passed to the JNI bindings.
+ private final int m_notifier = NotifierJNI.initializeNotifier();
+
+ // The absolute expiration time
+ private double m_expirationTime;
+
+ /**
+ * Constructor for TimedRobot.
+ */
+ protected TimedRobot() {
+ this(kDefaultPeriod);
+ }
+
+ /**
+ * Constructor for TimedRobot.
+ *
+ * @param period Period in seconds.
+ */
+ protected TimedRobot(double period) {
+ super(period);
+ NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
+
+ HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed);
+ }
+
+ @Override
+ @SuppressWarnings("NoFinalizer")
+ protected void finalize() {
+ NotifierJNI.stopNotifier(m_notifier);
+ NotifierJNI.cleanNotifier(m_notifier);
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ @Override
+ @SuppressWarnings("UnsafeFinalization")
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period;
+ updateAlarm();
+
+ // Loop forever, calling the appropriate mode-dependent function
+ while (true) {
+ long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
+ if (curTime == 0) {
+ break;
+ }
+
+ m_expirationTime += m_period;
+ updateAlarm();
+
+ loopFunc();
+ }
+ }
+
+ /**
+ * Ends the main loop in startCompetition().
+ */
+ @Override
+ public void endCompetition() {
+ NotifierJNI.stopNotifier(m_notifier);
+ }
+
+ /**
+ * Get time period between calls to Periodic() functions.
+ */
+ public double getPeriod() {
+ return m_period;
+ }
+
+ /**
+ * Update the alarm hardware to reflect the next alarm.
+ */
+ @SuppressWarnings("UnsafeFinalization")
+ private void updateAlarm() {
+ NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
new file mode 100644
index 0000000..e1e312f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+public class Timer {
+ /**
+ * Return the system clock time in seconds. Return the time from the FPGA hardware clock in
+ * seconds since the FPGA started.
+ *
+ * @return Robot running time in seconds.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static double getFPGATimestamp() {
+ return RobotController.getFPGATime() / 1000000.0;
+ }
+
+ /**
+ * Return the approximate match time. The FMS does not send an official match time to the robots,
+ * but does send an approximate match time. The value will count down the time remaining in the
+ * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
+ * dispute ref calls or guarantee that a function will trigger before the match ends) The
+ * Practice Match function of the DS approximates the behaviour seen on the field.
+ *
+ * @return Time remaining in current match period (auto or teleop) in seconds
+ */
+ public static double getMatchTime() {
+ return DriverStation.getInstance().getMatchTime();
+ }
+
+ /**
+ * Pause the thread for a specified time. Pause the execution of the thread for a specified period
+ * of time given in seconds. Motors will continue to run at their last assigned values, and
+ * sensors will continue to update. Only the task containing the wait will pause until the wait
+ * time is expired.
+ *
+ * @param seconds Length of time to pause
+ */
+ public static void delay(final double seconds) {
+ try {
+ Thread.sleep((long) (seconds * 1e3));
+ } catch (final InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ }
+
+ private double m_startTime;
+ private double m_accumulatedTime;
+ private boolean m_running;
+
+ @SuppressWarnings("JavadocMethod")
+ public Timer() {
+ reset();
+ }
+
+ private double getMsClock() {
+ return RobotController.getFPGATime() / 1000.0;
+ }
+
+ /**
+ * Get the current time from the timer. If the clock is running it is derived from the current
+ * system clock the start time stored in the timer class. If the clock is not running, then return
+ * the time when it was last stopped.
+ *
+ * @return Current time value for this timer in seconds
+ */
+ public synchronized double get() {
+ if (m_running) {
+ return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
+ } else {
+ return m_accumulatedTime;
+ }
+ }
+
+ /**
+ * Reset the timer by setting the time to 0. Make the timer startTime the current time so new
+ * requests will be relative now
+ */
+ public synchronized void reset() {
+ m_accumulatedTime = 0;
+ m_startTime = getMsClock();
+ }
+
+ /**
+ * Start the timer running. Just set the running flag to true indicating that all time requests
+ * should be relative to the system clock.
+ */
+ public synchronized void start() {
+ m_startTime = getMsClock();
+ m_running = true;
+ }
+
+ /**
+ * Stop the timer. This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than looking at the system
+ * clock.
+ */
+ public synchronized void stop() {
+ m_accumulatedTime = get();
+ m_running = false;
+ }
+
+ /**
+ * Check if the period specified has passed and if it has, advance the start time by that period.
+ * This is useful to decide if it's time to do periodic work without drifting later by the time it
+ * took to get around to checking.
+ *
+ * @param period The period to check for (in seconds).
+ * @return If the period has passed.
+ */
+ public synchronized boolean hasPeriodPassed(double period) {
+ 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 * 1000;
+ return true;
+ }
+ return false;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
new file mode 100644
index 0000000..db7454a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -0,0 +1,412 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Ultrasonic rangefinder class. The Ultrasonic rangefinder measures absolute distance based on the
+ * round-trip time of a ping generated by the controller. These sensors use two transducers, a
+ * speaker and a microphone both tuned to the ultrasonic range. A common ultrasonic sensor, the
+ * Daventech SRF04 requires a short pulse to be generated on a digital channel. This causes the
+ * chirp to be emitted. A second line becomes high as the ping is transmitted and goes low when the
+ * echo is received. The time that the line is high determines the round trip distance (time of
+ * flight).
+ */
+public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
+ /**
+ * The units to return when PIDGet is called.
+ */
+ public enum Unit {
+ /**
+ * Use inches for PIDGet.
+ */
+ kInches,
+ /**
+ * Use millimeters for PIDGet.
+ */
+ kMillimeters
+ }
+
+ // Time (sec) for the ping trigger pulse.
+ private static final double kPingTime = 10 * 1e-6;
+ private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
+ // ultrasonic sensor list
+ private static final List<Ultrasonic> m_sensors = new ArrayList<>();
+ // automatic round robin mode
+ private static volatile boolean m_automaticEnabled;
+ private DigitalInput m_echoChannel;
+ private DigitalOutput m_pingChannel;
+ private boolean m_allocatedChannels;
+ private boolean m_enabled;
+ private Counter m_counter;
+ // task doing the round-robin automatic sensing
+ private static Thread m_task;
+ private Unit m_units;
+ private static int m_instances;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ private SimDevice m_simDevice;
+ private SimBoolean m_simRangeValid;
+ private SimDouble m_simRange;
+
+ /**
+ * Background task that goes through the list of ultrasonic sensors and pings each one in turn.
+ * The counter is configured to read the timing of the returned echo pulse.
+ *
+ * <p><b>DANGER WILL ROBINSON, DANGER WILL ROBINSON:</b> This code runs as a task and assumes that
+ * none of the ultrasonic sensors will change while it's running. If one does, then this will
+ * certainly break. Make sure to disable automatic mode before changing anything with the
+ * sensors!!
+ */
+ private static class UltrasonicChecker extends Thread {
+ @Override
+ public synchronized void run() {
+ while (m_automaticEnabled) {
+ for (Ultrasonic sensor: m_sensors) {
+ if (!m_automaticEnabled) {
+ break;
+ }
+
+ if (sensor.isEnabled()) {
+ sensor.m_pingChannel.pulse(kPingTime); // do the ping
+ }
+
+ Timer.delay(0.1); // wait for ping to return
+ }
+ }
+ }
+ }
+
+ /**
+ * Initialize the Ultrasonic Sensor. This is the common code that initializes the ultrasonic
+ * sensor given that there are two digital I/O channels allocated. If the system was running in
+ * automatic mode (round robin) when the new sensor is added, it is stopped, the sensor is added,
+ * then automatic mode is restored.
+ */
+ private synchronized void initialize() {
+ m_simDevice = SimDevice.create("Ultrasonic", m_echoChannel.getChannel());
+ if (m_simDevice != null) {
+ m_simRangeValid = m_simDevice.createBoolean("Range Valid", false, true);
+ m_simRange = m_simDevice.createDouble("Range (in)", false, 0.0);
+ m_pingChannel.setSimDevice(m_simDevice);
+ m_echoChannel.setSimDevice(m_simDevice);
+ }
+ if (m_task == null) {
+ m_task = new UltrasonicChecker();
+ }
+ final boolean originalMode = m_automaticEnabled;
+ setAutomaticMode(false); // kill task when adding a new sensor
+ m_sensors.add(this);
+
+ m_counter = new Counter(m_echoChannel); // set up counter for this
+ SendableRegistry.addChild(this, m_counter);
+ // sensor
+ m_counter.setMaxPeriod(1.0);
+ m_counter.setSemiPeriodMode(true);
+ m_counter.reset();
+ m_enabled = true; // make it available for round robin scheduling
+ setAutomaticMode(originalMode);
+
+ m_instances++;
+ HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances);
+ SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel());
+ }
+
+ /**
+ * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
+ * and Vex ultrasonic sensors.
+ *
+ * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
+ * sending the ping.
+ * @param echoChannel The digital input channel that receives the echo. The length of time that
+ * the echo is high represents the round trip time of the ping, and the
+ * distance.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+ public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
+ m_pingChannel = new DigitalOutput(pingChannel);
+ m_echoChannel = new DigitalInput(echoChannel);
+ SendableRegistry.addChild(this, m_pingChannel);
+ SendableRegistry.addChild(this, m_echoChannel);
+ m_allocatedChannels = true;
+ m_units = units;
+ initialize();
+ }
+
+ /**
+ * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
+ * and Vex ultrasonic sensors. Default unit is inches.
+ *
+ * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
+ * sending the ping.
+ * @param echoChannel The digital input channel that receives the echo. The length of time that
+ * the echo is high represents the round trip time of the ping, and the
+ * distance.
+ */
+ public Ultrasonic(final int pingChannel, final int echoChannel) {
+ this(pingChannel, echoChannel, Unit.kInches);
+ }
+
+ /**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
+ * DigitalOutput for the ping channel.
+ *
+ * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
+ * 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to determine the
+ * range.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+ public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
+ requireNonNull(pingChannel, "Provided ping channel was null");
+ requireNonNull(echoChannel, "Provided echo channel was null");
+
+ m_allocatedChannels = false;
+ m_pingChannel = pingChannel;
+ m_echoChannel = echoChannel;
+ m_units = units;
+ initialize();
+ }
+
+ /**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
+ * DigitalOutput for the ping channel. Default unit is inches.
+ *
+ * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
+ * 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to determine the
+ * range.
+ */
+ public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
+ this(pingChannel, echoChannel, Unit.kInches);
+ }
+
+ /**
+ * Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing
+ * the allocated digital channels. If the system was in automatic mode (round robin), then it is
+ * stopped, then started again after this sensor is removed (provided this wasn't the last
+ * sensor).
+ */
+ @Override
+ public synchronized void close() {
+ SendableRegistry.remove(this);
+ final boolean wasAutomaticMode = m_automaticEnabled;
+ setAutomaticMode(false);
+ if (m_allocatedChannels) {
+ if (m_pingChannel != null) {
+ m_pingChannel.close();
+ }
+ if (m_echoChannel != null) {
+ m_echoChannel.close();
+ }
+ }
+
+ if (m_counter != null) {
+ m_counter.close();
+ m_counter = null;
+ }
+
+ m_pingChannel = null;
+ m_echoChannel = null;
+ synchronized (m_sensors) {
+ m_sensors.remove(this);
+ }
+ if (!m_sensors.isEmpty() && wasAutomaticMode) {
+ setAutomaticMode(true);
+ }
+
+ if (m_simDevice != null) {
+ m_simDevice.close();
+ m_simDevice = null;
+ }
+ }
+
+ /**
+ * Turn Automatic mode on/off. When in Automatic mode, all sensors will fire in round robin,
+ * waiting a set time between each sensor.
+ *
+ * @param enabling Set to true if round robin scheduling should start for all the ultrasonic
+ * sensors. This scheduling method assures that the sensors are non-interfering
+ * because no two sensors fire at the same time. If another scheduling algorithm
+ * is preferred, it can be implemented by pinging the sensors manually and waiting
+ * for the results to come back.
+ */
+ public void setAutomaticMode(boolean enabling) {
+ if (enabling == m_automaticEnabled) {
+ return; // ignore the case of no change
+ }
+ m_automaticEnabled = enabling;
+
+ if (enabling) {
+ /* Clear all the counters so no data is valid. No synchronization is
+ * needed because the background task is stopped.
+ */
+ for (Ultrasonic u : m_sensors) {
+ u.m_counter.reset();
+ }
+
+ // Start round robin task
+ m_task.start();
+ } else {
+ // Wait for background task to stop running
+ try {
+ m_task.join();
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ ex.printStackTrace();
+ }
+
+ /* Clear all the counters (data now invalid) since automatic mode is
+ * disabled. No synchronization is needed because the background task is
+ * stopped.
+ */
+ for (Ultrasonic u : m_sensors) {
+ u.m_counter.reset();
+ }
+ }
+ }
+
+ /**
+ * Single ping to ultrasonic sensor. Send out a single ping to the ultrasonic sensor. This only
+ * works if automatic (round robin) mode is disabled. A single ping is sent out, and the counter
+ * should count the semi-period when it comes in. The counter is reset to make the current value
+ * invalid.
+ */
+ public void ping() {
+ setAutomaticMode(false); // turn off automatic round robin if pinging
+ // single sensor
+ m_counter.reset(); // reset the counter to zero (invalid data now)
+ // do the ping to start getting a single range
+ m_pingChannel.pulse(kPingTime);
+ }
+
+ /**
+ * Check if there is a valid range measurement. The ranges are accumulated in a counter that will
+ * increment on each edge of the echo (return) signal. If the count is not at least 2, then the
+ * range has not yet been measured, and is invalid.
+ *
+ * @return true if the range is valid
+ */
+ public boolean isRangeValid() {
+ if (m_simRangeValid != null) {
+ return m_simRangeValid.get();
+ }
+ return m_counter.get() > 1;
+ }
+
+ /**
+ * Get the range in inches from the ultrasonic sensor. If there is no valid value yet, i.e. at
+ * least one measurement hasn't completed, then return 0.
+ *
+ * @return double Range in inches of the target returned from the ultrasonic sensor.
+ */
+ public double getRangeInches() {
+ if (isRangeValid()) {
+ if (m_simRange != null) {
+ return m_simRange.get();
+ }
+ return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
+ } else {
+ return 0;
+ }
+ }
+
+ /**
+ * Get the range in millimeters from the ultrasonic sensor. If there is no valid value yet, i.e.
+ * at least one measurement hasn't completed, then return 0.
+ *
+ * @return double Range in millimeters of the target returned by the ultrasonic sensor.
+ */
+ public double getRangeMM() {
+ return getRangeInches() * 25.4;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+ throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
+ }
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the range in the current DistanceUnit for the PIDSource base object.
+ *
+ * @return The range in DistanceUnit
+ */
+ @Override
+ public double pidGet() {
+ switch (m_units) {
+ case kInches:
+ return getRangeInches();
+ case kMillimeters:
+ return getRangeMM();
+ default:
+ return 0.0;
+ }
+ }
+
+ /**
+ * Set the current DistanceUnit that should be used for the PIDSource base object.
+ *
+ * @param units The DistanceUnit that should be used.
+ */
+ public void setDistanceUnits(Unit units) {
+ m_units = units;
+ }
+
+ /**
+ * Get the current DistanceUnit that is used for the PIDSource base object.
+ *
+ * @return The type of DistanceUnit that is being used.
+ */
+ public Unit getDistanceUnits() {
+ return m_units;
+ }
+
+ /**
+ * Is the ultrasonic enabled.
+ *
+ * @return true if the ultrasonic is enabled
+ */
+ public boolean isEnabled() {
+ return m_enabled;
+ }
+
+ /**
+ * Set if the ultrasonic is enabled.
+ *
+ * @param enable set to true to enable the ultrasonic
+ */
+ public void setEnabled(boolean enable) {
+ m_enabled = enable;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Ultrasonic");
+ builder.addDoubleProperty("Value", this::getRangeInches, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
new file mode 100644
index 0000000..2d19240
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * VEX Robotics Victor 888 Speed Controller The Vex Robotics Victor 884 Speed Controller can also
+ * be used with this class but may need to be calibrated per the Victor 884 user manual.
+ *
+ * <p>Note that the Victor uses the following bounds for PWM values. These values were determined
+ * empirically and optimized for the Victor 888. These values should work reasonably well for
+ * Victor 884 controllers also but if users experience issues such as asymmetric behaviour around
+ * the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User Manual available
+ * from VEX Robotics: http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf
+ *
+ * <p><ul>
+ * <li>2.027ms = full "forward"
+ * <li>1.525ms = the "high end" of the deadband range
+ * <li>1.507ms = center of the deadband range (off)
+ * <li>1.490ms = the "low end" of the deadband range
+ * <li>1.026ms = full "reverse"
+ * </ul>
+ */
+public class Victor extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the Victor is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ public Victor(final int channel) {
+ super(channel);
+
+ setBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+ setPeriodMultiplier(PeriodMultiplier.k2X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_Victor, getChannel() + 1);
+ SendableRegistry.setName(this, "Victor", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
new file mode 100644
index 0000000..22b3513
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * VEX Robotics Victor SP Speed Controller.
+ *
+ * <p>Note that the VictorSP uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the VictorSP User Manual
+ * available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class VictorSP extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ public VictorSP(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_VictorSP, getChannel() + 1);
+ SendableRegistry.setName(this, "VictorSP", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
new file mode 100644
index 0000000..223e992
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
@@ -0,0 +1,283 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.Closeable;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.PriorityQueue;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
+
+/**
+ * A class that's a wrapper around a watchdog timer.
+ *
+ * <p>When the timer expires, a message is printed to the console and an optional user-provided
+ * callback is invoked.
+ *
+ * <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class Watchdog implements Closeable, Comparable<Watchdog> {
+ // Used for timeout print rate-limiting
+ private static final long kMinPrintPeriod = 1000000; // us
+
+ private long m_startTime; // us
+ private long m_timeout; // us
+ private long m_expirationTime; // us
+ private final Runnable m_callback;
+ private long m_lastTimeoutPrintTime; // us
+ private long m_lastEpochsPrintTime; // us
+
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private final Map<String, Long> m_epochs = new HashMap<>();
+ boolean m_isExpired;
+
+ boolean m_suppressTimeoutMessage;
+
+ static {
+ startDaemonThread(Watchdog::schedulerFunc);
+ }
+
+ private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
+ private static ReentrantLock m_queueMutex = new ReentrantLock();
+ private static Condition m_schedulerWaiter = m_queueMutex.newCondition();
+
+ /**
+ * Watchdog constructor.
+ *
+ * @param timeout The watchdog's timeout in seconds with microsecond resolution.
+ * @param callback This function is called when the timeout expires.
+ */
+ public Watchdog(double timeout, Runnable callback) {
+ m_timeout = (long) (timeout * 1.0e6);
+ m_callback = callback;
+ }
+
+ @Override
+ public void close() {
+ disable();
+ }
+
+ @Override
+ public int compareTo(Watchdog rhs) {
+ // Elements with sooner expiration times are sorted as lesser. The head of
+ // Java's PriorityQueue is the least element.
+ return Long.compare(m_expirationTime, rhs.m_expirationTime);
+ }
+
+ /**
+ * Returns the time in seconds since the watchdog was last fed.
+ */
+ public double getTime() {
+ return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
+ }
+
+ /**
+ * Sets the watchdog's timeout.
+ *
+ * @param timeout The watchdog's timeout in seconds with microsecond
+ * resolution.
+ */
+ public void setTimeout(double timeout) {
+ m_startTime = RobotController.getFPGATime();
+ m_epochs.clear();
+
+ m_queueMutex.lock();
+ try {
+ m_timeout = (long) (timeout * 1.0e6);
+ m_isExpired = false;
+
+ m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + m_timeout;
+ m_watchdogs.add(this);
+ m_schedulerWaiter.signalAll();
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the watchdog's timeout in seconds.
+ */
+ public double getTimeout() {
+ m_queueMutex.lock();
+ try {
+ return m_timeout / 1.0e6;
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns true if the watchdog timer has expired.
+ */
+ public boolean isExpired() {
+ m_queueMutex.lock();
+ try {
+ return m_isExpired;
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Adds time since last epoch to the list printed by printEpochs().
+ *
+ * <p>Epochs are a way to partition the time elapsed so that when overruns occur, one can
+ * determine which parts of an operation consumed the most time.
+ *
+ * @param epochName The name to associate with the epoch.
+ */
+ public void addEpoch(String epochName) {
+ long currentTime = RobotController.getFPGATime();
+ m_epochs.put(epochName, currentTime - m_startTime);
+ m_startTime = currentTime;
+ }
+
+ /**
+ * Prints list of epochs added so far and their times.
+ */
+ public void printEpochs() {
+ long now = RobotController.getFPGATime();
+ if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+ m_lastEpochsPrintTime = now;
+ m_epochs.forEach((key, value) -> System.out.format("\t%s: %.6fs\n", key, value / 1.0e6));
+ }
+ }
+
+ /**
+ * Resets the watchdog timer.
+ *
+ * <p>This also enables the timer if it was previously disabled.
+ */
+ public void reset() {
+ enable();
+ }
+
+ /**
+ * Enables the watchdog timer.
+ */
+ public void enable() {
+ m_startTime = RobotController.getFPGATime();
+ m_epochs.clear();
+
+ m_queueMutex.lock();
+ try {
+ m_isExpired = false;
+
+ m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + m_timeout;
+ m_watchdogs.add(this);
+ m_schedulerWaiter.signalAll();
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Disables the watchdog timer.
+ */
+ public void disable() {
+ m_queueMutex.lock();
+ try {
+ m_watchdogs.remove(this);
+ m_schedulerWaiter.signalAll();
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Enable or disable suppression of the generic timeout message.
+ *
+ * <p>This may be desirable if the user-provided callback already prints a more specific message.
+ *
+ * @param suppress Whether to suppress generic timeout message.
+ */
+ public void suppressTimeoutMessage(boolean suppress) {
+ m_suppressTimeoutMessage = suppress;
+ }
+
+ private static Thread startDaemonThread(Runnable target) {
+ Thread inst = new Thread(target);
+ inst.setDaemon(true);
+ inst.start();
+ return inst;
+ }
+
+
+ @SuppressWarnings("PMD.AvoidDeeplyNestedIfStmts")
+ private static void schedulerFunc() {
+ m_queueMutex.lock();
+
+ try {
+ while (!Thread.currentThread().isInterrupted()) {
+ if (m_watchdogs.size() > 0) {
+ boolean timedOut = !awaitUntil(m_schedulerWaiter, m_watchdogs.peek().m_expirationTime);
+ if (timedOut) {
+ if (m_watchdogs.size() == 0 || m_watchdogs.peek().m_expirationTime
+ > RobotController.getFPGATime()) {
+ continue;
+ }
+
+ // If the condition variable timed out, that means a Watchdog timeout
+ // has occurred, so call its timeout function.
+ Watchdog watchdog = m_watchdogs.poll();
+
+ long now = RobotController.getFPGATime();
+ if (now - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
+ watchdog.m_lastTimeoutPrintTime = now;
+ if (!watchdog.m_suppressTimeoutMessage) {
+ System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
+ }
+ }
+
+ // Set expiration flag before calling the callback so any
+ // manipulation of the flag in the callback (e.g., calling
+ // Disable()) isn't clobbered.
+ watchdog.m_isExpired = true;
+
+ m_queueMutex.unlock();
+ watchdog.m_callback.run();
+ m_queueMutex.lock();
+ }
+ // Otherwise, a Watchdog removed itself from the queue (it notifies
+ // the scheduler of this) or a spurious wakeup occurred, so just
+ // rewait with the soonest watchdog timeout.
+ } else {
+ while (m_watchdogs.size() == 0) {
+ m_schedulerWaiter.awaitUninterruptibly();
+ }
+ }
+ }
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Wrapper emulating functionality of C++'s std::condition_variable::wait_until().
+ *
+ * @param cond The condition variable on which to wait.
+ * @param time The time at which to stop waiting.
+ * @return False if the deadline has elapsed upon return, else true.
+ */
+ private static boolean awaitUntil(Condition cond, long time) {
+ long delta = time - RobotController.getFPGATime();
+ try {
+ return cond.await(delta, TimeUnit.MICROSECONDS);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ ex.printStackTrace();
+ }
+
+ return true;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
new file mode 100644
index 0000000..fd6161a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -0,0 +1,345 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Handle input from Xbox 360 or Xbox One controllers connected to the Driver Station.
+ *
+ * <p>This class handles Xbox input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each controller
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class XboxController extends GenericHID {
+ /**
+ * Represents a digital button on an XboxController.
+ */
+ public enum Button {
+ kBumperLeft(5),
+ kBumperRight(6),
+ kStickLeft(9),
+ kStickRight(10),
+ kA(1),
+ kB(2),
+ kX(3),
+ kY(4),
+ kBack(7),
+ kStart(8);
+
+ @SuppressWarnings({"MemberName", "PMD.SingularField"})
+ public final int value;
+
+ Button(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Construct an instance of a joystick. The joystick index is the USB port on the drivers
+ * station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into.
+ */
+ public XboxController(final int port) {
+ super(port);
+
+ HAL.report(tResourceType.kResourceType_XboxController, port + 1);
+ }
+
+ /**
+ * Get the X axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The X axis value of the controller.
+ */
+ @Override
+ public double getX(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawAxis(0);
+ } else {
+ return getRawAxis(4);
+ }
+ }
+
+ /**
+ * Get the Y axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The Y axis value of the controller.
+ */
+ @Override
+ public double getY(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawAxis(1);
+ } else {
+ return getRawAxis(5);
+ }
+ }
+
+ /**
+ * Get the trigger axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The trigger axis value of the controller.
+ */
+ public double getTriggerAxis(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawAxis(2);
+ } else {
+ return getRawAxis(3);
+ }
+ }
+
+ /**
+ * Read the value of the bumper button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+ public boolean getBumper(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawButton(Button.kBumperLeft.value);
+ } else {
+ return getRawButton(Button.kBumperRight.value);
+ }
+ }
+
+ /**
+ * Whether the bumper was pressed since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getBumperPressed(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonPressed(Button.kBumperLeft.value);
+ } else {
+ return getRawButtonPressed(Button.kBumperRight.value);
+ }
+ }
+
+ /**
+ * Whether the bumper was released since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getBumperReleased(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonReleased(Button.kBumperLeft.value);
+ } else {
+ return getRawButtonReleased(Button.kBumperRight.value);
+ }
+ }
+
+ /**
+ * Read the value of the stick button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+ public boolean getStickButton(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawButton(Button.kStickLeft.value);
+ } else {
+ return getRawButton(Button.kStickRight.value);
+ }
+ }
+
+ /**
+ * Whether the stick button was pressed since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getStickButtonPressed(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonPressed(Button.kStickLeft.value);
+ } else {
+ return getRawButtonPressed(Button.kStickRight.value);
+ }
+ }
+
+ /**
+ * Whether the stick button was released since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getStickButtonReleased(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonReleased(Button.kStickLeft.value);
+ } else {
+ return getRawButtonReleased(Button.kStickRight.value);
+ }
+ }
+
+ /**
+ * Read the value of the A button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getAButton() {
+ return getRawButton(Button.kA.value);
+ }
+
+ /**
+ * Whether the A button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getAButtonPressed() {
+ return getRawButtonPressed(Button.kA.value);
+ }
+
+ /**
+ * Whether the A button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getAButtonReleased() {
+ return getRawButtonReleased(Button.kA.value);
+ }
+
+ /**
+ * Read the value of the B button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getBButton() {
+ return getRawButton(Button.kB.value);
+ }
+
+ /**
+ * Whether the B button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getBButtonPressed() {
+ return getRawButtonPressed(Button.kB.value);
+ }
+
+ /**
+ * Whether the B button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getBButtonReleased() {
+ return getRawButtonReleased(Button.kB.value);
+ }
+
+ /**
+ * Read the value of the X button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getXButton() {
+ return getRawButton(Button.kX.value);
+ }
+
+ /**
+ * Whether the X button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getXButtonPressed() {
+ return getRawButtonPressed(Button.kX.value);
+ }
+
+ /**
+ * Whether the X button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getXButtonReleased() {
+ return getRawButtonReleased(Button.kX.value);
+ }
+
+ /**
+ * Read the value of the Y button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getYButton() {
+ return getRawButton(Button.kY.value);
+ }
+
+ /**
+ * Whether the Y button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getYButtonPressed() {
+ return getRawButtonPressed(Button.kY.value);
+ }
+
+ /**
+ * Whether the Y button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getYButtonReleased() {
+ return getRawButtonReleased(Button.kY.value);
+ }
+
+ /**
+ * Read the value of the back button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getBackButton() {
+ return getRawButton(Button.kBack.value);
+ }
+
+ /**
+ * Whether the back button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getBackButtonPressed() {
+ return getRawButtonPressed(Button.kBack.value);
+ }
+
+ /**
+ * Whether the back button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getBackButtonReleased() {
+ return getRawButtonReleased(Button.kBack.value);
+ }
+
+ /**
+ * Read the value of the start button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getStartButton() {
+ return getRawButton(Button.kStart.value);
+ }
+
+ /**
+ * Whether the start button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getStartButtonPressed() {
+ return getRawButtonPressed(Button.kStart.value);
+ }
+
+ /**
+ * Whether the start button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getStartButtonReleased() {
+ return getRawButtonReleased(Button.kStart.value);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
new file mode 100644
index 0000000..33f9784
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as a motor
+ * acting against the force of gravity on a beam suspended at an angle).
+ */
+@SuppressWarnings("MemberName")
+public class ArmFeedforward {
+ public final double ks;
+ public final double kcos;
+ public final double kv;
+ public final double ka;
+
+ /**
+ * Creates a new ArmFeedforward with the specified gains. Units of the gain values
+ * will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kcos The gravity gain.
+ * @param kv The velocity gain.
+ * @param ka The acceleration gain.
+ */
+ public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+ this.ks = ks;
+ this.kcos = kcos;
+ this.kv = kv;
+ this.ka = ka;
+ }
+
+ /**
+ * Creates a new ArmFeedforward with the specified gains. Acceleration gain is
+ * defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kcos The gravity gain.
+ * @param kv The velocity gain.
+ */
+ public ArmFeedforward(double ks, double kcos, double kv) {
+ this(ks, kcos, kv, 0);
+ }
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param positionRadians The position (angle) setpoint.
+ * @param velocityRadPerSec The velocity setpoint.
+ * @param accelRadPerSecSquared The acceleration setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double positionRadians, double velocityRadPerSec,
+ double accelRadPerSecSquared) {
+ return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
+ + kv * velocityRadPerSec
+ + ka * accelRadPerSecSquared;
+ }
+
+ /**
+ * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+ * be zero).
+ *
+ * @param positionRadians The position (angle) setpoint.
+ * @param velocity The velocity setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double positionRadians, double velocity) {
+ return calculate(positionRadians, velocity, 0);
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply,
+ * a position, and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm.
+ * @param acceleration The acceleration of the arm.
+ * @return The maximum possible velocity at the given acceleration and angle.
+ */
+ public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply,
+ * a position, and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm.
+ * @param acceleration The acceleration of the arm.
+ * @return The minimum possible velocity at the given acceleration and angle.
+ */
+ public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply, a position, and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm.
+ * @param velocity The velocity of the arm.
+ * @return The maximum possible acceleration at the given velocity.
+ */
+ public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+ return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
+ }
+
+ /**
+ * Calculates the minimum achievable acceleration given a maximum voltage
+ * supply, a position, and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm.
+ * @param velocity The velocity of the arm.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+ return maxAchievableAcceleration(-maxVoltage, angle, velocity);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
new file mode 100644
index 0000000..86a4f7e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
+ * acting against the force of gravity).
+ */
+@SuppressWarnings("MemberName")
+public class ElevatorFeedforward {
+ public final double ks;
+ public final double kg;
+ public final double kv;
+ public final double ka;
+
+ /**
+ * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values
+ * will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kg The gravity gain.
+ * @param kv The velocity gain.
+ * @param ka The acceleration gain.
+ */
+ public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+ this.ks = ks;
+ this.kg = kg;
+ this.kv = kv;
+ this.ka = ka;
+ }
+
+ /**
+ * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is
+ * defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kg The gravity gain.
+ * @param kv The velocity gain.
+ */
+ public ElevatorFeedforward(double ks, double kg, double kv) {
+ this(ks, kg, kv, 0);
+ }
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint.
+ * @param acceleration The acceleration setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double velocity, double acceleration) {
+ return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
+ }
+
+ /**
+ * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+ * be zero).
+ *
+ * @param velocity The velocity setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double velocity) {
+ return calculate(velocity, 0);
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param acceleration The acceleration of the elevator.
+ * @return The maximum possible velocity at the given acceleration.
+ */
+ public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - ks - kg - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param acceleration The acceleration of the elevator.
+ * @return The minimum possible velocity at the given acceleration.
+ */
+ public double minAchievableVelocity(double maxVoltage, double acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + ks - kg - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param velocity The velocity of the elevator.
+ * @return The maximum possible acceleration at the given velocity.
+ */
+ public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+ return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
+ }
+
+ /**
+ * Calculates the minimum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param velocity The velocity of the elevator.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ public double minAchievableAcceleration(double maxVoltage, double velocity) {
+ return maxAchievableAcceleration(-maxVoltage, velocity);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
new file mode 100644
index 0000000..eea0644
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -0,0 +1,384 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Implements a PID control loop.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class PIDController implements Sendable, AutoCloseable {
+ private static int instances;
+
+ // Factor for "proportional" control
+ @SuppressWarnings("MemberName")
+ private double m_Kp;
+
+ // Factor for "integral" control
+ @SuppressWarnings("MemberName")
+ private double m_Ki;
+
+ // Factor for "derivative" control
+ @SuppressWarnings("MemberName")
+ private double m_Kd;
+
+ // The period (in seconds) of the loop that calls the controller
+ private final double m_period;
+
+ private double m_maximumIntegral = 1.0;
+
+ private double m_minimumIntegral = -1.0;
+
+ // Maximum input - limit setpoint to this
+ private double m_maximumInput;
+
+ // Minimum input - limit setpoint to this
+ private double m_minimumInput;
+
+ // Input range - difference between maximum and minimum
+ private double m_inputRange;
+
+ // Do the endpoints wrap around? eg. Absolute encoder
+ private boolean m_continuous;
+
+ // The error at the time of the most recent call to calculate()
+ private double m_positionError;
+ private double m_velocityError;
+
+ // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
+ private double m_prevError;
+
+ // The sum of the errors for use in the integral calc
+ private double m_totalError;
+
+ // The percentage or absolute error that is considered at setpoint.
+ private double m_positionTolerance = 0.05;
+ private double m_velocityTolerance = Double.POSITIVE_INFINITY;
+
+ private double m_setpoint;
+
+ /**
+ * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
+ * 0.02 seconds.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd) {
+ this(Kp, Ki, Kd, 0.02);
+ }
+
+ /**
+ * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ * @param period The period between controller updates in seconds.
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, double period) {
+ m_Kp = Kp;
+ m_Ki = Ki;
+ m_Kd = Kd;
+
+ m_period = period;
+
+ instances++;
+ SendableRegistry.addLW(this, "PIDController", instances);
+
+ HAL.report(tResourceType.kResourceType_PIDController2, instances);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Sets the PID Controller gain parameters.
+ *
+ * <p>Set the proportional, integral, and differential coefficients.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ */
+ @SuppressWarnings("ParameterName")
+ public void setPID(double Kp, double Ki, double Kd) {
+ m_Kp = Kp;
+ m_Ki = Ki;
+ m_Kd = Kd;
+ }
+
+ /**
+ * Sets the Proportional coefficient of the PID controller gain.
+ *
+ * @param Kp proportional coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setP(double Kp) {
+ m_Kp = Kp;
+ }
+
+ /**
+ * Sets the Integral coefficient of the PID controller gain.
+ *
+ * @param Ki integral coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setI(double Ki) {
+ m_Ki = Ki;
+ }
+
+ /**
+ * Sets the Differential coefficient of the PID controller gain.
+ *
+ * @param Kd differential coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setD(double Kd) {
+ m_Kd = Kd;
+ }
+
+ /**
+ * Get the Proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+ public double getP() {
+ return m_Kp;
+ }
+
+ /**
+ * Get the Integral coefficient.
+ *
+ * @return integral coefficient
+ */
+ public double getI() {
+ return m_Ki;
+ }
+
+ /**
+ * Get the Differential coefficient.
+ *
+ * @return differential coefficient
+ */
+ public double getD() {
+ return m_Kd;
+ }
+
+ /**
+ * Returns the period of this controller.
+ *
+ * @return the period of the controller.
+ */
+ public double getPeriod() {
+ return m_period;
+ }
+
+ /**
+ * Sets the setpoint for the PIDController.
+ *
+ * @param setpoint The desired setpoint.
+ */
+ public void setSetpoint(double setpoint) {
+ if (m_maximumInput > m_minimumInput) {
+ m_setpoint = MathUtil.clamp(setpoint, m_minimumInput, m_maximumInput);
+ } else {
+ m_setpoint = setpoint;
+ }
+ }
+
+ /**
+ * Returns the current setpoint of the PIDController.
+ *
+ * @return The current setpoint.
+ */
+ public double getSetpoint() {
+ return m_setpoint;
+ }
+
+ /**
+ * Returns true if the error is within the percentage of the total input range, determined by
+ * SetTolerance. This asssumes that the maximum and minimum input were set using SetInput.
+ *
+ * <p>This will return false until at least one input value has been computed.
+ *
+ * @return Whether the error is within the acceptable bounds.
+ */
+ public boolean atSetpoint() {
+ return Math.abs(m_positionError) < m_positionTolerance
+ && Math.abs(m_velocityError) < m_velocityTolerance;
+ }
+
+ /**
+ * Enables continuous input.
+ *
+ * <p>Rather then using the max and min input range as constraints, it considers
+ * them to be the same point and automatically calculates the shortest route
+ * to the setpoint.
+ *
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+ public void enableContinuousInput(double minimumInput, double maximumInput) {
+ m_continuous = true;
+ setInputRange(minimumInput, maximumInput);
+ }
+
+ /**
+ * Disables continuous input.
+ */
+ public void disableContinuousInput() {
+ m_continuous = false;
+ }
+
+ /**
+ * Sets the minimum and maximum values for the integrator.
+ *
+ * <p>When the cap is reached, the integrator value is added to the controller
+ * output rather than the integrator value times the integral gain.
+ *
+ * @param minimumIntegral The minimum value of the integrator.
+ * @param maximumIntegral The maximum value of the integrator.
+ */
+ public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+ m_minimumIntegral = minimumIntegral;
+ m_maximumIntegral = maximumIntegral;
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with atSetpoint().
+ *
+ * @param positionTolerance Position error which is tolerable.
+ */
+ public void setTolerance(double positionTolerance) {
+ setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with atSetpoint().
+ *
+ * @param positionTolerance Position error which is tolerable.
+ * @param velocityTolerance Velocity error which is tolerable.
+ */
+ public void setTolerance(double positionTolerance, double velocityTolerance) {
+ m_positionTolerance = positionTolerance;
+ m_velocityTolerance = velocityTolerance;
+ }
+
+ /**
+ * Returns the difference between the setpoint and the measurement.
+ *
+ * @return The error.
+ */
+ public double getPositionError() {
+ return getContinuousError(m_positionError);
+ }
+
+ /**
+ * Returns the velocity error.
+ */
+ public double getVelocityError() {
+ return m_velocityError;
+ }
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param setpoint The new setpoint of the controller.
+ */
+ public double calculate(double measurement, double setpoint) {
+ // Set setpoint to provided value
+ setSetpoint(setpoint);
+ return calculate(measurement);
+ }
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ */
+ public double calculate(double measurement) {
+ m_prevError = m_positionError;
+ m_positionError = getContinuousError(m_setpoint - measurement);
+ m_velocityError = (m_positionError - m_prevError) / m_period;
+
+ if (m_Ki != 0) {
+ m_totalError = MathUtil.clamp(m_totalError + m_positionError * m_period,
+ m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+ }
+
+ return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+ }
+
+ /**
+ * Resets the previous error and the integral term.
+ */
+ public void reset() {
+ m_prevError = 0;
+ m_totalError = 0;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PIDController");
+ builder.addDoubleProperty("p", this::getP, this::setP);
+ builder.addDoubleProperty("i", this::getI, this::setI);
+ builder.addDoubleProperty("d", this::getD, this::setD);
+ builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+ }
+
+ /**
+ * Wraps error around for continuous inputs. The original error is returned if continuous mode is
+ * disabled.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+ protected double getContinuousError(double error) {
+ if (m_continuous && m_inputRange > 0) {
+ error %= m_inputRange;
+ if (Math.abs(error) > m_inputRange / 2) {
+ if (error > 0) {
+ return error - m_inputRange;
+ } else {
+ return error + m_inputRange;
+ }
+ }
+ }
+ return error;
+ }
+
+ /**
+ * Sets the minimum and maximum values expected from the input.
+ *
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+ private void setInputRange(double minimumInput, double maximumInput) {
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+
+ // Clamp setpoint to new input
+ if (m_maximumInput > m_minimumInput) {
+ m_setpoint = MathUtil.clamp(m_setpoint, m_minimumInput, m_maximumInput);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
new file mode 100644
index 0000000..7550aa5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
@@ -0,0 +1,360 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class ProfiledPIDController implements Sendable {
+ private static int instances;
+
+ private PIDController m_controller;
+ private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+ private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+ private TrapezoidProfile.Constraints m_constraints;
+
+ /**
+ * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+ * Kd.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ * @param constraints Velocity and acceleration constraints for goal.
+ */
+ @SuppressWarnings("ParameterName")
+ public ProfiledPIDController(double Kp, double Ki, double Kd,
+ TrapezoidProfile.Constraints constraints) {
+ this(Kp, Ki, Kd, constraints, 0.02);
+ }
+
+ /**
+ * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+ * Kd.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ * @param constraints Velocity and acceleration constraints for goal.
+ * @param period The period between controller updates in seconds. The
+ * default is 0.02 seconds.
+ */
+ @SuppressWarnings("ParameterName")
+ public ProfiledPIDController(double Kp, double Ki, double Kd,
+ TrapezoidProfile.Constraints constraints,
+ double period) {
+ m_controller = new PIDController(Kp, Ki, Kd, period);
+ m_constraints = constraints;
+ instances++;
+ HAL.report(tResourceType.kResourceType_ProfiledPIDController, instances);
+ }
+
+ /**
+ * Sets the PID Controller gain parameters.
+ *
+ * <p>Sets the proportional, integral, and differential coefficients.
+ *
+ * @param Kp Proportional coefficient
+ * @param Ki Integral coefficient
+ * @param Kd Differential coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setPID(double Kp, double Ki, double Kd) {
+ m_controller.setPID(Kp, Ki, Kd);
+ }
+
+ /**
+ * Sets the proportional coefficient of the PID controller gain.
+ *
+ * @param Kp proportional coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setP(double Kp) {
+ m_controller.setP(Kp);
+ }
+
+ /**
+ * Sets the integral coefficient of the PID controller gain.
+ *
+ * @param Ki integral coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setI(double Ki) {
+ m_controller.setI(Ki);
+ }
+
+ /**
+ * Sets the differential coefficient of the PID controller gain.
+ *
+ * @param Kd differential coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setD(double Kd) {
+ m_controller.setD(Kd);
+ }
+
+ /**
+ * Gets the proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+ public double getP() {
+ return m_controller.getP();
+ }
+
+ /**
+ * Gets the integral coefficient.
+ *
+ * @return integral coefficient
+ */
+ public double getI() {
+ return m_controller.getI();
+ }
+
+ /**
+ * Gets the differential coefficient.
+ *
+ * @return differential coefficient
+ */
+ public double getD() {
+ return m_controller.getD();
+ }
+
+ /**
+ * Gets the period of this controller.
+ *
+ * @return The period of the controller.
+ */
+ public double getPeriod() {
+ return m_controller.getPeriod();
+ }
+
+ /**
+ * Sets the goal for the ProfiledPIDController.
+ *
+ * @param goal The desired goal state.
+ */
+ public void setGoal(TrapezoidProfile.State goal) {
+ m_goal = goal;
+ }
+
+ /**
+ * Sets the goal for the ProfiledPIDController.
+ *
+ * @param goal The desired goal position.
+ */
+ public void setGoal(double goal) {
+ m_goal = new TrapezoidProfile.State(goal, 0);
+ }
+
+ /**
+ * Gets the goal for the ProfiledPIDController.
+ */
+ public TrapezoidProfile.State getGoal() {
+ return m_goal;
+ }
+
+ /**
+ * Returns true if the error is within the tolerance of the error.
+ *
+ * <p>This will return false until at least one input value has been computed.
+ */
+ public boolean atGoal() {
+ return atSetpoint() && m_goal.equals(m_setpoint);
+ }
+
+ /**
+ * Set velocity and acceleration constraints for goal.
+ *
+ * @param constraints Velocity and acceleration constraints for goal.
+ */
+ public void setConstraints(TrapezoidProfile.Constraints constraints) {
+ m_constraints = constraints;
+ }
+
+ /**
+ * Returns the current setpoint of the ProfiledPIDController.
+ *
+ * @return The current setpoint.
+ */
+ public TrapezoidProfile.State getSetpoint() {
+ return m_setpoint;
+ }
+
+ /**
+ * Returns true if the error is within the tolerance of the error.
+ *
+ * <p>This will return false until at least one input value has been computed.
+ */
+ public boolean atSetpoint() {
+ return m_controller.atSetpoint();
+ }
+
+ /**
+ * Enables continuous input.
+ *
+ * <p>Rather then using the max and min input range as constraints, it considers
+ * them to be the same point and automatically calculates the shortest route
+ * to the setpoint.
+ *
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+ public void enableContinuousInput(double minimumInput, double maximumInput) {
+ m_controller.enableContinuousInput(minimumInput, maximumInput);
+ }
+
+ /**
+ * Disables continuous input.
+ */
+ public void disableContinuousInput() {
+ m_controller.disableContinuousInput();
+ }
+
+ /**
+ * Sets the minimum and maximum values for the integrator.
+ *
+ * <p>When the cap is reached, the integrator value is added to the controller
+ * output rather than the integrator value times the integral gain.
+ *
+ * @param minimumIntegral The minimum value of the integrator.
+ * @param maximumIntegral The maximum value of the integrator.
+ */
+ public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+ m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with atSetpoint().
+ *
+ * @param positionTolerance Position error which is tolerable.
+ */
+ public void setTolerance(double positionTolerance) {
+ setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with atSetpoint().
+ *
+ * @param positionTolerance Position error which is tolerable.
+ * @param velocityTolerance Velocity error which is tolerable.
+ */
+ public void setTolerance(double positionTolerance, double velocityTolerance) {
+ m_controller.setTolerance(positionTolerance, velocityTolerance);
+ }
+
+ /**
+ * Returns the difference between the setpoint and the measurement.
+ *
+ * @return The error.
+ */
+ public double getPositionError() {
+ return m_controller.getPositionError();
+ }
+
+ /**
+ * Returns the change in error per second.
+ */
+ public double getVelocityError() {
+ return m_controller.getVelocityError();
+ }
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ */
+ public double calculate(double measurement) {
+ var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
+ m_setpoint = profile.calculate(getPeriod());
+ return m_controller.calculate(measurement, m_setpoint.position);
+ }
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param goal The new goal of the controller.
+ */
+ public double calculate(double measurement, TrapezoidProfile.State goal) {
+ setGoal(goal);
+ return calculate(measurement);
+ }
+
+ /**
+ * Returns the next output of the PIDController.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param goal The new goal of the controller.
+ */
+ public double calculate(double measurement, double goal) {
+ setGoal(goal);
+ return calculate(measurement);
+ }
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param goal The new goal of the controller.
+ * @param constraints Velocity and acceleration constraints for goal.
+ */
+ public double calculate(double measurement, TrapezoidProfile.State goal,
+ TrapezoidProfile.Constraints constraints) {
+ setConstraints(constraints);
+ return calculate(measurement, goal);
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measurement The current measured State of the system.
+ */
+ public void reset(TrapezoidProfile.State measurement) {
+ m_controller.reset();
+ m_setpoint = measurement;
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system.
+ * @param measuredVelocity The current measured velocity of the system.
+ */
+ public void reset(double measuredPosition, double measuredVelocity) {
+ reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system. The velocity is
+ * assumed to be zero.
+ */
+ public void reset(double measuredPosition) {
+ reset(measuredPosition, 0.0);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("ProfiledPIDController");
+ builder.addDoubleProperty("p", this::getP, this::setP);
+ builder.addDoubleProperty("i", this::getI, this::setI);
+ builder.addDoubleProperty("d", this::getD, this::setD);
+ builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
new file mode 100644
index 0000000..3ff9ac5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models
+ * that drives the model to a desired pose along a two-dimensional trajectory.
+ * Why would we need a nonlinear control law in addition to the linear ones we
+ * have used so far like PID? If we use the original approach with PID
+ * controllers for left and right position and velocity states, the controllers
+ * only deal with the local pose. If the robot deviates from the path, there is
+ * no way for the controllers to correct and the robot may not reach the desired
+ * global pose. This is due to multiple endpoints existing for the robot which
+ * have the same encoder path arc lengths.
+ *
+ * <p>Instead of using wheel path arc lengths (which are in the robot's local
+ * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
+ * global pose. The controller uses this extra information to guide a linear
+ * reference tracker like the PID controllers back in by adjusting the
+ * references of the PID controllers.
+ *
+ * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
+ * describes a nonlinear controller for a wheeled vehicle with unicycle-like
+ * kinematics; a global pose consisting of x, y, and theta; and a desired pose
+ * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
+ * acronym for the title of the book it came from in Italian ("Robotica
+ * Articolata e Mobile per i SErvizi e le TEcnologie").
+ *
+ * <p>See
+ * <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">
+ * Controls Engineering in the FIRST Robotics Competition</a> section on Ramsete
+ * unicycle controller for a derivation and analysis.
+ */
+public class RamseteController {
+ @SuppressWarnings("MemberName")
+ private final double m_b;
+ @SuppressWarnings("MemberName")
+ private final double m_zeta;
+
+ private Pose2d m_poseError = new Pose2d();
+ private Pose2d m_poseTolerance = new Pose2d();
+
+ /**
+ * Construct a Ramsete unicycle controller.
+ *
+ * @param b Tuning parameter (b > 0) for which larger values make convergence more
+ * aggressive like a proportional term.
+ * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide more damping
+ * in response.
+ */
+ @SuppressWarnings("ParameterName")
+ public RamseteController(double b, double zeta) {
+ m_b = b;
+ m_zeta = zeta;
+ }
+
+ /**
+ * Construct a Ramsete unicycle controller. The default arguments for
+ * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+ * results.
+ */
+ public RamseteController() {
+ this(2.0, 0.7);
+ }
+
+ /**
+ * Returns true if the pose error is within tolerance of the reference.
+ */
+ public boolean atReference() {
+ final var eTranslate = m_poseError.getTranslation();
+ final var eRotate = m_poseError.getRotation();
+ final var tolTranslate = m_poseTolerance.getTranslation();
+ final var tolRotate = m_poseTolerance.getRotation();
+ return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+ && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+ && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+ }
+
+ /**
+ * Sets the pose error which is considered tolerable for use with
+ * atReference().
+ *
+ * @param poseTolerance Pose error which is tolerable.
+ */
+ public void setTolerance(Pose2d poseTolerance) {
+ m_poseTolerance = poseTolerance;
+ }
+
+ /**
+ * Returns the next output of the Ramsete controller.
+ *
+ * <p>The reference pose, linear velocity, and angular velocity should come
+ * from a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param poseRef The desired pose.
+ * @param linearVelocityRefMeters The desired linear velocity in meters.
+ * @param angularVelocityRefRadiansPerSecond The desired angular velocity in meters.
+ */
+ @SuppressWarnings("LocalVariableName")
+ public ChassisSpeeds calculate(Pose2d currentPose,
+ Pose2d poseRef,
+ double linearVelocityRefMeters,
+ double angularVelocityRefRadiansPerSecond) {
+ m_poseError = poseRef.relativeTo(currentPose);
+
+ // Aliases for equation readability
+ final double eX = m_poseError.getTranslation().getX();
+ final double eY = m_poseError.getTranslation().getY();
+ final double eTheta = m_poseError.getRotation().getRadians();
+ final double vRef = linearVelocityRefMeters;
+ final double omegaRef = angularVelocityRefRadiansPerSecond;
+
+ double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
+
+ return new ChassisSpeeds(vRef * m_poseError.getRotation().getCos() + k * eX,
+ 0.0,
+ omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
+ }
+
+ /**
+ * Returns the next output of the Ramsete controller.
+ *
+ * <p>The reference pose, linear velocity, and angular velocity should come
+ * from a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param desiredState The desired pose, linear velocity, and angular velocity
+ * from a trajectory.
+ */
+ @SuppressWarnings("LocalVariableName")
+ public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
+ return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond,
+ desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
+ }
+
+ /**
+ * Returns sin(x) / x.
+ *
+ * @param x Value of which to take sinc(x).
+ */
+ @SuppressWarnings("ParameterName")
+ private static double sinc(double x) {
+ if (Math.abs(x) < 1e-9) {
+ return 1.0 - 1.0 / 6.0 * x * x;
+ } else {
+ return Math.sin(x) / x;
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
new file mode 100644
index 0000000..9d2c57e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
+ */
+@SuppressWarnings("MemberName")
+public class SimpleMotorFeedforward {
+ public final double ks;
+ public final double kv;
+ public final double ka;
+
+ /**
+ * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values
+ * will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kv The velocity gain.
+ * @param ka The acceleration gain.
+ */
+ public SimpleMotorFeedforward(double ks, double kv, double ka) {
+ this.ks = ks;
+ this.kv = kv;
+ this.ka = ka;
+ }
+
+ /**
+ * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is
+ * defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
+ *
+ * @param ks The static gain.
+ * @param kv The velocity gain.
+ */
+ public SimpleMotorFeedforward(double ks, double kv) {
+ this(ks, kv, 0);
+ }
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint.
+ * @param acceleration The acceleration setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double velocity, double acceleration) {
+ return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+ * be zero).
+ *
+ * @param velocity The velocity setpoint.
+ * @return The computed feedforward.
+ */
+ public double calculate(double velocity) {
+ return calculate(velocity, 0);
+ }
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The maximum possible velocity at the given acceleration.
+ */
+ public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - ks - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The minimum possible velocity at the given acceleration.
+ */
+ public double minAchievableVelocity(double maxVoltage, double acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + ks - acceleration * ka) / kv;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param velocity The velocity of the motor.
+ * @return The maximum possible acceleration at the given velocity.
+ */
+ public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+ return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param velocity The velocity of the motor.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ public double minAchievableAcceleration(double maxVoltage, double velocity) {
+ return maxAchievableAcceleration(-maxVoltage, velocity);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
new file mode 100644
index 0000000..cbfd3c0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -0,0 +1,436 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import java.util.StringJoiner;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
+ * base, "tank drive", or West Coast Drive.
+ *
+ * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
+ * (e.g., 6WD or 8WD). This class takes a SpeedController per side. For four and
+ * six motor drivetrains, construct and pass in {@link edu.wpi.first.wpilibj.SpeedControllerGroup}
+ * instances as follows.
+ *
+ * <p>Four motor drivetrain:
+ * <pre><code>
+ * public class Robot {
+ * SpeedController m_frontLeft = new PWMVictorSPX(1);
+ * SpeedController m_rearLeft = new PWMVictorSPX(2);
+ * SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
+ *
+ * SpeedController m_frontRight = new PWMVictorSPX(3);
+ * SpeedController m_rearRight = new PWMVictorSPX(4);
+ * SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
+ *
+ * DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ * </code></pre>
+ *
+ * <p>Six motor drivetrain:
+ * <pre><code>
+ * public class Robot {
+ * SpeedController m_frontLeft = new PWMVictorSPX(1);
+ * SpeedController m_midLeft = new PWMVictorSPX(2);
+ * SpeedController m_rearLeft = new PWMVictorSPX(3);
+ * SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
+ *
+ * SpeedController m_frontRight = new PWMVictorSPX(4);
+ * SpeedController m_midRight = new PWMVictorSPX(5);
+ * SpeedController m_rearRight = new PWMVictorSPX(6);
+ * SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
+ *
+ * DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ * </code></pre>
+ *
+ * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
+ *
+ * <p>Drive base diagram:
+ * <pre>
+ * |_______|
+ * | | | |
+ * | |
+ * |_|___|_|
+ * | |
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a differential drive
+ * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is
+ * usually unnecessary.
+ *
+ * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
+ * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
+ *
+ * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
+ * positive.
+ *
+ * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
+ * be set to 0, and larger values will be scaled so that the full range is still used. This
+ * deadband value can be changed with {@link #setDeadband}.
+ *
+ * <p>RobotDrive porting guide:
+ * <br>{@link #tankDrive(double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used.
+ * <br>{@link #arcadeDrive(double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used
+ * and the the rotation input is inverted eg arcadeDrive(y, -rotation)
+ * <br>{@link #curvatureDrive(double, double, boolean)} is similar in concept to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn
+ * mode. However, it is not designed to give exactly the same response.
+ */
+public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
+ public static final double kDefaultQuickStopThreshold = 0.2;
+ public static final double kDefaultQuickStopAlpha = 0.1;
+
+ private static int instances;
+
+ private final SpeedController m_leftMotor;
+ private final SpeedController m_rightMotor;
+
+ private double m_quickStopThreshold = kDefaultQuickStopThreshold;
+ private double m_quickStopAlpha = kDefaultQuickStopAlpha;
+ private double m_quickStopAccumulator;
+ private double m_rightSideInvertMultiplier = -1.0;
+ private boolean m_reported;
+
+ /**
+ * Construct a DifferentialDrive.
+ *
+ * <p>To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be
+ * inverted, do so before passing it in.
+ */
+ public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) {
+ verify(leftMotor, rightMotor);
+ m_leftMotor = leftMotor;
+ m_rightMotor = rightMotor;
+ SendableRegistry.addChild(this, m_leftMotor);
+ SendableRegistry.addChild(this, m_rightMotor);
+ instances++;
+ SendableRegistry.addLW(this, "DifferentialDrive", instances);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
+ * The exception's error message will specify all null motors, e.g. {@code
+ * NullPointerException("leftMotor, rightMotor")}, to give as much information as possible to
+ * the programmer.
+ *
+ * @throws NullPointerException if any of the given motors are null
+ */
+ @SuppressWarnings("PMD.AvoidThrowingNullPointerException")
+ private void verify(SpeedController leftMotor, SpeedController rightMotor) {
+ if (leftMotor != null && rightMotor != null) {
+ return;
+ }
+ StringJoiner joiner = new StringJoiner(", ");
+ if (leftMotor == null) {
+ joiner.add("leftMotor");
+ }
+ if (rightMotor == null) {
+ joiner.add("rightMotor");
+ }
+ throw new NullPointerException(joiner.toString());
+ }
+
+ /**
+ * Arcade drive method for differential drive platform.
+ * The calculated values will be squared to decrease sensitivity at low speeds.
+ *
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void arcadeDrive(double xSpeed, double zRotation) {
+ arcadeDrive(xSpeed, zRotation, true);
+ }
+
+ /**
+ * Arcade drive method for differential drive platform.
+ *
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param squareInputs If set, decreases the input sensitivity at low speeds.
+ */
+ @SuppressWarnings("ParameterName")
+ public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive,
+ tInstances.kRobotDrive2_DifferentialArcade, 2);
+ m_reported = true;
+ }
+
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+ zRotation = applyDeadband(zRotation, m_deadband);
+
+ // Square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power.
+ if (squareInputs) {
+ xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
+ zRotation = Math.copySign(zRotation * zRotation, zRotation);
+ }
+
+ double leftMotorOutput;
+ double rightMotorOutput;
+
+ double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
+
+ if (xSpeed >= 0.0) {
+ // First quadrant, else second quadrant
+ if (zRotation >= 0.0) {
+ leftMotorOutput = maxInput;
+ rightMotorOutput = xSpeed - zRotation;
+ } else {
+ leftMotorOutput = xSpeed + zRotation;
+ rightMotorOutput = maxInput;
+ }
+ } else {
+ // Third quadrant, else fourth quadrant
+ if (zRotation >= 0.0) {
+ leftMotorOutput = xSpeed + zRotation;
+ rightMotorOutput = maxInput;
+ } else {
+ leftMotorOutput = maxInput;
+ rightMotorOutput = xSpeed - zRotation;
+ }
+ }
+
+ m_leftMotor.set(MathUtil.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
+ double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
+ m_rightMotor.set(MathUtil.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Curvature drive method for differential drive platform.
+ *
+ * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
+ * heading change. This makes the robot more controllable at high speeds. Also handles the
+ * robot's quick turn functionality - "quick turn" overrides constant-curvature turning for
+ * turn-in-place maneuvers.
+ *
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param isQuickTurn If set, overrides constant-curvature turning for
+ * turn-in-place maneuvers.
+ */
+ @SuppressWarnings({"ParameterName", "PMD.CyclomaticComplexity"})
+ public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive,
+ tInstances.kRobotDrive2_DifferentialCurvature, 2);
+ m_reported = true;
+ }
+
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+ zRotation = applyDeadband(zRotation, m_deadband);
+
+ double angularPower;
+ boolean overPower;
+
+ if (isQuickTurn) {
+ if (Math.abs(xSpeed) < m_quickStopThreshold) {
+ m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator
+ + m_quickStopAlpha * MathUtil.clamp(zRotation, -1.0, 1.0) * 2;
+ }
+ overPower = true;
+ angularPower = zRotation;
+ } else {
+ overPower = false;
+ angularPower = Math.abs(xSpeed) * zRotation - m_quickStopAccumulator;
+
+ if (m_quickStopAccumulator > 1) {
+ m_quickStopAccumulator -= 1;
+ } else if (m_quickStopAccumulator < -1) {
+ m_quickStopAccumulator += 1;
+ } else {
+ m_quickStopAccumulator = 0.0;
+ }
+ }
+
+ double leftMotorOutput = xSpeed + angularPower;
+ double rightMotorOutput = xSpeed - angularPower;
+
+ // If rotation is overpowered, reduce both outputs to within acceptable range
+ if (overPower) {
+ if (leftMotorOutput > 1.0) {
+ rightMotorOutput -= leftMotorOutput - 1.0;
+ leftMotorOutput = 1.0;
+ } else if (rightMotorOutput > 1.0) {
+ leftMotorOutput -= rightMotorOutput - 1.0;
+ rightMotorOutput = 1.0;
+ } else if (leftMotorOutput < -1.0) {
+ rightMotorOutput -= leftMotorOutput + 1.0;
+ leftMotorOutput = -1.0;
+ } else if (rightMotorOutput < -1.0) {
+ leftMotorOutput -= rightMotorOutput + 1.0;
+ rightMotorOutput = -1.0;
+ }
+ }
+
+ // Normalize the wheel speeds
+ double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
+ if (maxMagnitude > 1.0) {
+ leftMotorOutput /= maxMagnitude;
+ rightMotorOutput /= maxMagnitude;
+ }
+
+ m_leftMotor.set(leftMotorOutput * m_maxOutput);
+ m_rightMotor.set(rightMotorOutput * m_maxOutput * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Tank drive method for differential drive platform.
+ * The calculated values will be squared to decrease sensitivity at low speeds.
+ *
+ * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ */
+ public void tankDrive(double leftSpeed, double rightSpeed) {
+ tankDrive(leftSpeed, rightSpeed, true);
+ }
+
+ /**
+ * Tank drive method for differential drive platform.
+ *
+ * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ * @param squareInputs If set, decreases the input sensitivity at low speeds.
+ */
+ public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive,
+ tInstances.kRobotDrive2_DifferentialTank, 2);
+ m_reported = true;
+ }
+
+ leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
+ leftSpeed = applyDeadband(leftSpeed, m_deadband);
+
+ rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
+ rightSpeed = applyDeadband(rightSpeed, m_deadband);
+
+ // Square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power.
+ if (squareInputs) {
+ leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
+ rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
+ }
+
+ m_leftMotor.set(leftSpeed * m_maxOutput);
+ m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Sets the QuickStop speed threshold in curvature drive.
+ *
+ * <p>QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn.
+ *
+ * <p>While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value
+ * outputted by the low-pass filter when the robot's speed along the X axis is below the
+ * threshold. When QuickTurn is disabled, the accumulator's value is applied against the computed
+ * angular power request to slow the robot's rotation.
+ *
+ * @param threshold X speed below which quick stop accumulator will receive rotation rate values
+ * [0..1.0].
+ */
+ public void setQuickStopThreshold(double threshold) {
+ m_quickStopThreshold = threshold;
+ }
+
+ /**
+ * Sets the low-pass filter gain for QuickStop in curvature drive.
+ *
+ * <p>The low-pass filter filters incoming rotation rate commands to smooth out high frequency
+ * changes.
+ *
+ * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes.
+ * Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and
+ * above 2.0 are unstable.
+ */
+ public void setQuickStopAlpha(double alpha) {
+ m_quickStopAlpha = alpha;
+ }
+
+ /**
+ * Gets if the power sent to the right side of the drivetrain is multipled by -1.
+ *
+ * @return true if the right side is inverted
+ */
+ public boolean isRightSideInverted() {
+ return m_rightSideInvertMultiplier == -1.0;
+ }
+
+ /**
+ * Sets if the power sent to the right side of the drivetrain should be multipled by -1.
+ *
+ * @param rightSideInverted true if right side power should be multipled by -1
+ */
+ public void setRightSideInverted(boolean rightSideInverted) {
+ m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+ }
+
+ @Override
+ public void stopMotor() {
+ m_leftMotor.stopMotor();
+ m_rightMotor.stopMotor();
+ feed();
+ }
+
+ @Override
+ public String getDescription() {
+ return "DifferentialDrive";
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("DifferentialDrive");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
+ builder.addDoubleProperty(
+ "Right Motor Speed",
+ () -> m_rightMotor.get() * m_rightSideInvertMultiplier,
+ x -> m_rightMotor.set(x * m_rightSideInvertMultiplier));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
new file mode 100644
index 0000000..45da78a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import java.util.StringJoiner;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * A class for driving Killough drive platforms.
+ *
+ * <p>Killough drives are triangular with one omni wheel on each corner.
+ *
+ * <p>Drive base diagram:
+ * <pre>
+ * /_____\
+ * / \ / \
+ * \ /
+ * ---
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a Killough drive. The
+ * default wheel vectors are parallel to their respective opposite sides, but can be overridden. See
+ * the constructor for more information.
+ *
+ * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
+ * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
+ *
+ * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
+ * positive.
+ */
+public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable {
+ public static final double kDefaultLeftMotorAngle = 60.0;
+ public static final double kDefaultRightMotorAngle = 120.0;
+ public static final double kDefaultBackMotorAngle = 270.0;
+
+ private static int instances;
+
+ private SpeedController m_leftMotor;
+ private SpeedController m_rightMotor;
+ private SpeedController m_backMotor;
+
+ private Vector2d m_leftVec;
+ private Vector2d m_rightVec;
+ private Vector2d m_backVec;
+
+ private boolean m_reported;
+
+ /**
+ * Construct a Killough drive with the given motors and default motor angles.
+ *
+ * <p>The default motor angles make the wheels on each corner parallel to their respective
+ * opposite sides.
+ *
+ * <p>If a motor needs to be inverted, do so before passing it in.
+ *
+ * @param leftMotor The motor on the left corner.
+ * @param rightMotor The motor on the right corner.
+ * @param backMotor The motor on the back corner.
+ */
+ public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
+ SpeedController backMotor) {
+ this(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle, kDefaultRightMotorAngle,
+ kDefaultBackMotorAngle);
+ }
+
+ /**
+ * Construct a Killough drive with the given motors.
+ *
+ * <p>Angles are measured in degrees clockwise from the positive X axis.
+ *
+ * @param leftMotor The motor on the left corner.
+ * @param rightMotor The motor on the right corner.
+ * @param backMotor The motor on the back corner.
+ * @param leftMotorAngle The angle of the left wheel's forward direction of travel.
+ * @param rightMotorAngle The angle of the right wheel's forward direction of travel.
+ * @param backMotorAngle The angle of the back wheel's forward direction of travel.
+ */
+ public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
+ SpeedController backMotor, double leftMotorAngle, double rightMotorAngle,
+ double backMotorAngle) {
+ verify(leftMotor, rightMotor, backMotor);
+ m_leftMotor = leftMotor;
+ m_rightMotor = rightMotor;
+ m_backMotor = backMotor;
+ m_leftVec = new Vector2d(Math.cos(leftMotorAngle * (Math.PI / 180.0)),
+ Math.sin(leftMotorAngle * (Math.PI / 180.0)));
+ m_rightVec = new Vector2d(Math.cos(rightMotorAngle * (Math.PI / 180.0)),
+ Math.sin(rightMotorAngle * (Math.PI / 180.0)));
+ m_backVec = new Vector2d(Math.cos(backMotorAngle * (Math.PI / 180.0)),
+ Math.sin(backMotorAngle * (Math.PI / 180.0)));
+ SendableRegistry.addChild(this, m_leftMotor);
+ SendableRegistry.addChild(this, m_rightMotor);
+ SendableRegistry.addChild(this, m_backMotor);
+ instances++;
+ SendableRegistry.addLW(this, "KilloughDrive", instances);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
+ * The exception's error message will specify all null motors, e.g. {@code
+ * NullPointerException("leftMotor, rightMotor")}, to give as much information as possible to
+ * the programmer.
+ *
+ * @throws NullPointerException if any of the given motors are null
+ */
+ @SuppressWarnings("PMD.AvoidThrowingNullPointerException")
+ private void verify(SpeedController leftMotor, SpeedController rightMotor,
+ SpeedController backMotor) {
+ if (leftMotor != null && rightMotor != null && backMotor != null) {
+ return;
+ }
+ StringJoiner joiner = new StringJoiner(", ");
+ if (leftMotor == null) {
+ joiner.add("leftMotor");
+ }
+ if (rightMotor == null) {
+ joiner.add("rightMotor");
+ }
+ if (backMotor == null) {
+ joiner.add("backMotor");
+ }
+ throw new NullPointerException(joiner.toString());
+ }
+
+ /**
+ * Drive method for Killough platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
+ driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
+ }
+
+ /**
+ * Drive method for Killough platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use
+ * this to implement field-oriented controls.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation,
+ double gyroAngle) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive,
+ tInstances.kRobotDrive2_KilloughCartesian, 3);
+ m_reported = true;
+ }
+
+ ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+ ySpeed = applyDeadband(ySpeed, m_deadband);
+
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ // Compensate for gyro angle.
+ Vector2d input = new Vector2d(ySpeed, xSpeed);
+ input.rotate(-gyroAngle);
+
+ double[] wheelSpeeds = new double[3];
+ wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation;
+ wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation;
+ wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation;
+
+ normalize(wheelSpeeds);
+
+ m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput);
+ m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput);
+ m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Drive method for Killough platform.
+ *
+ * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
+ * drives (translation) is independent from its angle or rotation rate.
+ *
+ * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
+ * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drivePolar(double magnitude, double angle, double zRotation) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive,
+ tInstances.kRobotDrive2_KilloughPolar, 3);
+ m_reported = true;
+ }
+
+ driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
+ magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
+ }
+
+ @Override
+ public void stopMotor() {
+ m_leftMotor.stopMotor();
+ m_rightMotor.stopMotor();
+ m_backMotor.stopMotor();
+ feed();
+ }
+
+ @Override
+ public String getDescription() {
+ return "KilloughDrive";
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("KilloughDrive");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
+ builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set);
+ builder.addDoubleProperty("Back Motor Speed", m_backMotor::get, m_backMotor::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
new file mode 100644
index 0000000..702bdf5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -0,0 +1,267 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import java.util.StringJoiner;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * A class for driving Mecanum drive platforms.
+ *
+ * <p>Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in
+ * 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles
+ * should form an X across the robot. Each drive() function provides different inverse kinematic
+ * relations for a Mecanum drive robot.
+ *
+ * <p>Drive base diagram:
+ * <pre>
+ * \\_______/
+ * \\ | | /
+ * | |
+ * /_|___|_\\
+ * / \\
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
+ * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is
+ * usually unnecessary.
+ *
+ * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
+ * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
+ *
+ * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
+ * positive.
+ *
+ * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
+ * be set to 0, and larger values will be scaled so that the full range is still used. This
+ * deadband value can be changed with {@link #setDeadband}.
+ *
+ * <p>RobotDrive porting guide:
+ * <br>In MecanumDrive, the right side speed controllers are automatically inverted, while in
+ * RobotDrive, no speed controllers are automatically inverted.
+ * <br>{@link #driveCartesian(double, double, double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Cartesian(double, double, double, double)}
+ * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted compared to
+ * RobotDrive (eg driveCartesian(xSpeed, -ySpeed, zRotation, -gyroAngle).
+ * <br>{@link #drivePolar(double, double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Polar(double, double, double)} if a
+ * deadband of 0 is used.
+ */
+public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
+ private static int instances;
+
+ private final SpeedController m_frontLeftMotor;
+ private final SpeedController m_rearLeftMotor;
+ private final SpeedController m_frontRightMotor;
+ private final SpeedController m_rearRightMotor;
+
+ private double m_rightSideInvertMultiplier = -1.0;
+ private boolean m_reported;
+
+ /**
+ * Construct a MecanumDrive.
+ *
+ * <p>If a motor needs to be inverted, do so before passing it in.
+ */
+ public MecanumDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
+ SpeedController frontRightMotor, SpeedController rearRightMotor) {
+ verify(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
+ m_frontLeftMotor = frontLeftMotor;
+ m_rearLeftMotor = rearLeftMotor;
+ m_frontRightMotor = frontRightMotor;
+ m_rearRightMotor = rearRightMotor;
+ SendableRegistry.addChild(this, m_frontLeftMotor);
+ SendableRegistry.addChild(this, m_rearLeftMotor);
+ SendableRegistry.addChild(this, m_frontRightMotor);
+ SendableRegistry.addChild(this, m_rearRightMotor);
+ instances++;
+ SendableRegistry.addLW(this, "MecanumDrive", instances);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
+ * The exception's error message will specify all null motors, e.g. {@code
+ * NullPointerException("frontLeftMotor, rearRightMotor")}, to give as much information as
+ * possible to the programmer.
+ *
+ * @throws NullPointerException if any of the given motors are null
+ */
+ @SuppressWarnings({"PMD.AvoidThrowingNullPointerException", "PMD.CyclomaticComplexity"})
+ private void verify(SpeedController frontLeft, SpeedController rearLeft,
+ SpeedController frontRight, SpeedController rearRightMotor) {
+ if (frontLeft != null && rearLeft != null && frontRight != null && rearRightMotor != null) {
+ return;
+ }
+ StringJoiner joiner = new StringJoiner(", ");
+ if (frontLeft == null) {
+ joiner.add("frontLeftMotor");
+ }
+ if (rearLeft == null) {
+ joiner.add("rearLeftMotor");
+ }
+ if (frontRight == null) {
+ joiner.add("frontRightMotor");
+ }
+ if (rearRightMotor == null) {
+ joiner.add("rearRightMotor");
+ }
+ throw new NullPointerException(joiner.toString());
+ }
+
+ /**
+ * Drive method for Mecanum platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
+ driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
+ }
+
+ /**
+ * Drive method for Mecanum platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use
+ * this to implement field-oriented controls.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive,
+ tInstances.kRobotDrive2_MecanumCartesian, 4);
+ m_reported = true;
+ }
+
+ ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+ ySpeed = applyDeadband(ySpeed, m_deadband);
+
+ xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ // Compensate for gyro angle.
+ Vector2d input = new Vector2d(ySpeed, xSpeed);
+ input.rotate(-gyroAngle);
+
+ double[] wheelSpeeds = new double[4];
+ wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
+ wheelSpeeds[MotorType.kFrontRight.value] = -input.x + input.y - zRotation;
+ wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
+ wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
+
+ normalize(wheelSpeeds);
+
+ m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
+ m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput
+ * m_rightSideInvertMultiplier);
+ m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
+ m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput
+ * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Drive method for Mecanum platform.
+ *
+ * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
+ * drives (translation) is independent from its angle or rotation rate.
+ *
+ * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
+ * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drivePolar(double magnitude, double angle, double zRotation) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumPolar, 4);
+ m_reported = true;
+ }
+
+ driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
+ magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
+ }
+
+ /**
+ * Gets if the power sent to the right side of the drivetrain is multipled by -1.
+ *
+ * @return true if the right side is inverted
+ */
+ public boolean isRightSideInverted() {
+ return m_rightSideInvertMultiplier == -1.0;
+ }
+
+ /**
+ * Sets if the power sent to the right side of the drivetrain should be multipled by -1.
+ *
+ * @param rightSideInverted true if right side power should be multipled by -1
+ */
+ public void setRightSideInverted(boolean rightSideInverted) {
+ m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+ }
+
+ @Override
+ public void stopMotor() {
+ m_frontLeftMotor.stopMotor();
+ m_frontRightMotor.stopMotor();
+ m_rearLeftMotor.stopMotor();
+ m_rearRightMotor.stopMotor();
+ feed();
+ }
+
+ @Override
+ public String getDescription() {
+ return "MecanumDrive";
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("MecanumDrive");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Front Left Motor Speed",
+ m_frontLeftMotor::get,
+ m_frontLeftMotor::set);
+ builder.addDoubleProperty("Front Right Motor Speed",
+ () -> m_frontRightMotor.get() * m_rightSideInvertMultiplier,
+ value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier));
+ builder.addDoubleProperty("Rear Left Motor Speed",
+ m_rearLeftMotor::get,
+ m_rearLeftMotor::set);
+ builder.addDoubleProperty("Rear Right Motor Speed",
+ () -> m_rearRightMotor.get() * m_rightSideInvertMultiplier,
+ value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
new file mode 100644
index 0000000..785a0c5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import edu.wpi.first.wpilibj.MotorSafety;
+
+/**
+ * Common base class for drive platforms.
+ */
+public abstract class RobotDriveBase extends MotorSafety {
+ public static final double kDefaultDeadband = 0.02;
+ public static final double kDefaultMaxOutput = 1.0;
+
+ protected double m_deadband = kDefaultDeadband;
+ protected double m_maxOutput = kDefaultMaxOutput;
+
+ /**
+ * The location of a motor on the robot for the purpose of driving.
+ */
+ public enum MotorType {
+ kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3), kLeft(0),
+ kRight(1), kBack(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ MotorType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * RobotDriveBase constructor.
+ */
+ public RobotDriveBase() {
+ setSafetyEnabled(true);
+ }
+
+ /**
+ * Sets the deadband applied to the drive inputs (e.g., joystick values).
+ *
+ * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
+ * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See
+ * {@link #applyDeadband}.
+ *
+ * @param deadband The deadband to set.
+ */
+ public void setDeadband(double deadband) {
+ m_deadband = deadband;
+ }
+
+ /**
+ * Configure the scaling factor for using drive methods with motor controllers in a mode other
+ * than PercentVbus or to limit the maximum output.
+ *
+ * <p>The default value is {@value #kDefaultMaxOutput}.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_maxOutput = maxOutput;
+ }
+
+ /**
+ * Feed the motor safety object. Resets the timer that will stop the motors if it completes.
+ *
+ * @see MotorSafety#feed()
+ */
+ public void feedWatchdog() {
+ feed();
+ }
+
+ @Override
+ public abstract void stopMotor();
+
+ @Override
+ public abstract String getDescription();
+
+ /**
+ * Returns 0.0 if the given value is within the specified range around zero. The remaining range
+ * between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ *
+ * @param value value to clip
+ * @param deadband range around zero
+ */
+ protected double applyDeadband(double value, double deadband) {
+ if (Math.abs(value) > deadband) {
+ if (value > 0.0) {
+ return (value - deadband) / (1.0 - deadband);
+ } else {
+ return (value + deadband) / (1.0 - deadband);
+ }
+ } else {
+ return 0.0;
+ }
+ }
+
+ /**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+ protected void normalize(double[] wheelSpeeds) {
+ double maxMagnitude = Math.abs(wheelSpeeds[0]);
+ for (int i = 1; i < wheelSpeeds.length; i++) {
+ double temp = Math.abs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) {
+ maxMagnitude = temp;
+ }
+ }
+ if (maxMagnitude > 1.0) {
+ for (int i = 0; i < wheelSpeeds.length; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
new file mode 100644
index 0000000..2b1f839
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+/**
+ * This is a 2D vector struct that supports basic vector operations.
+ */
+@SuppressWarnings("MemberName")
+public class Vector2d {
+ public double x;
+ public double y;
+
+ public Vector2d() {}
+
+ @SuppressWarnings("ParameterName")
+ public Vector2d(double x, double y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * Rotate a vector in Cartesian space.
+ *
+ * @param angle angle in degrees by which to rotate vector counter-clockwise.
+ */
+ public void rotate(double angle) {
+ double cosA = Math.cos(angle * (Math.PI / 180.0));
+ double sinA = Math.sin(angle * (Math.PI / 180.0));
+ double[] out = new double[2];
+ out[0] = x * cosA - y * sinA;
+ out[1] = x * sinA + y * cosA;
+ x = out[0];
+ y = out[1];
+ }
+
+ /**
+ * Returns dot product of this vector with argument.
+ *
+ * @param vec Vector with which to perform dot product.
+ */
+ public double dot(Vector2d vec) {
+ return x * vec.x + y * vec.y;
+ }
+
+ /**
+ * Returns magnitude of vector.
+ */
+ public double magnitude() {
+ return Math.sqrt(x * x + y * y);
+ }
+
+ /**
+ * Returns scalar projection of this vector onto argument.
+ *
+ * @param vec Vector onto which to project this vector.
+ */
+ public double scalarProject(Vector2d vec) {
+ return dot(vec) / vec.magnitude();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
new file mode 100644
index 0000000..257d36a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.filters;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/**
+ * Superclass for filters.
+ *
+ * @deprecated This class is no longer used.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public abstract class Filter implements PIDSource {
+ private final PIDSource m_source;
+
+ public Filter(PIDSource source) {
+ m_source = source;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_source.setPIDSourceType(pidSource);
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_source.getPIDSourceType();
+ }
+
+ @Override
+ public abstract double pidGet();
+
+ /**
+ * Returns the current filter estimate without also inserting new data as pidGet() would do.
+ *
+ * @return The current filter estimate
+ */
+ public abstract double get();
+
+ /**
+ * Reset the filter state.
+ */
+ public abstract void reset();
+
+ /**
+ * Calls PIDGet() of source.
+ *
+ * @return Current value of source
+ */
+ protected double pidGetSource() {
+ return m_source.pidGet();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
new file mode 100644
index 0000000..cbeb4aa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.filters;
+
+import java.util.Arrays;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
+ * Static factory methods are provided to create commonly used types of filters.
+ *
+ * <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
+ * a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ * <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
+ * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
+ * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
+ * impact of these high frequency components. Likewise, a "high pass" filter gets rid of
+ * slow-moving signal components, letting you detect large changes more easily.
+ *
+ * <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
+ * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
+ * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
+ * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
+ * PID controller out of this class!
+ *
+ * <p>For more on filters, I highly recommend the following articles: http://en.wikipedia
+ * .org/wiki/Linear_filter http://en.wikipedia.org/wiki/Iir_filter http://en.wikipedia
+ * .org/wiki/Fir_filter
+ *
+ * <p>Note 1: PIDGet() should be called by the user on a known, regular period. You can set up a
+ * Notifier to do this (look at the WPILib PIDController class), or do it "inline" with code in a
+ * periodic function.
+ *
+ * <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
+ * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
+ * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
+ * to make sure PIDGet() gets called at the desired, constant frequency!
+ *
+ * @deprecated Use LinearFilter class instead.
+ */
+@Deprecated
+public class LinearDigitalFilter extends Filter {
+ private static int instances;
+
+ private final CircularBuffer m_inputs;
+ private final CircularBuffer m_outputs;
+ private final double[] m_inputGains;
+ private final double[] m_outputGains;
+
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+ public LinearDigitalFilter(PIDSource source, double[] ffGains,
+ double[] fbGains) {
+ super(source);
+ m_inputs = new CircularBuffer(ffGains.length);
+ m_outputs = new CircularBuffer(fbGains.length);
+ m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+ m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+ instances++;
+ HAL.report(tResourceType.kResourceType_LinearFilter, instances);
+ }
+
+ /**
+ * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
+ * gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ * <p>This filter is stable for time constants greater than zero.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+ public static LinearDigitalFilter singlePoleIIR(PIDSource source,
+ double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {1.0 - gain};
+ double[] fbGains = {-gain};
+
+ return new LinearDigitalFilter(source, ffGains, fbGains);
+ }
+
+ /**
+ * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
+ * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ * <p>This filter is stable for time constants greater than zero.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+ public static LinearDigitalFilter highPass(PIDSource source,
+ double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {gain, -gain};
+ double[] fbGains = {-gain};
+
+ return new LinearDigitalFilter(source, ffGains, fbGains);
+ }
+
+ /**
+ * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
+ * x[0]).
+ *
+ * <p>This filter is always stable.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param taps The number of samples to average over. Higher = smoother but slower
+ * @throws IllegalArgumentException if number of taps is less than 1
+ */
+ public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
+ if (taps <= 0) {
+ throw new IllegalArgumentException("Number of taps was not at least 1");
+ }
+
+ double[] ffGains = new double[taps];
+ for (int i = 0; i < ffGains.length; i++) {
+ ffGains[i] = 1.0 / taps;
+ }
+
+ double[] fbGains = new double[0];
+
+ return new LinearDigitalFilter(source, ffGains, fbGains);
+ }
+
+ @Override
+ public double get() {
+ double retVal = 0.0;
+
+ // Calculate the new value
+ for (int i = 0; i < m_inputGains.length; i++) {
+ retVal += m_inputs.get(i) * m_inputGains[i];
+ }
+ for (int i = 0; i < m_outputGains.length; i++) {
+ retVal -= m_outputs.get(i) * m_outputGains[i];
+ }
+
+ return retVal;
+ }
+
+ @Override
+ public void reset() {
+ m_inputs.clear();
+ m_outputs.clear();
+ }
+
+ /**
+ * Calculates the next value of the filter.
+ *
+ * @return The filtered value at this step
+ */
+ @Override
+ public double pidGet() {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.addFirst(pidGetSource());
+
+ // Calculate the new value
+ for (int i = 0; i < m_inputGains.length; i++) {
+ retVal += m_inputs.get(i) * m_inputGains[i];
+ }
+ for (int i = 0; i < m_outputGains.length; i++) {
+ retVal -= m_outputs.get(i) * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.addFirst(retVal);
+
+ return retVal;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
new file mode 100644
index 0000000..1fa2c8d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
@@ -0,0 +1,271 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+@JsonSerialize(using = Pose2d.PoseSerializer.class)
+@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
+public class Pose2d {
+ private final Translation2d m_translation;
+ private final Rotation2d m_rotation;
+
+ /**
+ * Constructs a pose at the origin facing toward the positive X axis.
+ * (Translation2d{0, 0} and Rotation{0})
+ */
+ public Pose2d() {
+ m_translation = new Translation2d();
+ m_rotation = new Rotation2d();
+ }
+
+ /**
+ * Constructs a pose with the specified translation and rotation.
+ *
+ * @param translation The translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ public Pose2d(Translation2d translation, Rotation2d rotation) {
+ m_translation = translation;
+ m_rotation = rotation;
+ }
+
+ /**
+ * Convenience constructors that takes in x and y values directly instead of
+ * having to construct a Translation2d.
+ *
+ * @param x The x component of the translational component of the pose.
+ * @param y The y component of the translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ @SuppressWarnings("ParameterName")
+ public Pose2d(double x, double y, Rotation2d rotation) {
+ m_translation = new Translation2d(x, y);
+ m_rotation = rotation;
+ }
+
+ /**
+ * Transforms the pose by the given transformation and returns the new
+ * transformed pose.
+ *
+ * <p>The matrix multiplication is as follows
+ * [x_new] [cos, -sin, 0][transform.x]
+ * [y_new] += [sin, cos, 0][transform.y]
+ * [t_new] [0, 0, 1][transform.t]
+ *
+ * @param other The transform to transform the pose by.
+ * @return The transformed pose.
+ */
+ public Pose2d plus(Transform2d other) {
+ return transformBy(other);
+ }
+
+ /**
+ * Returns the Transform2d that maps the one pose to another.
+ *
+ * @param other The initial pose of the transformation.
+ * @return The transform that maps the other pose to the current pose.
+ */
+ public Transform2d minus(Pose2d other) {
+ final var pose = this.relativeTo(other);
+ return new Transform2d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return The translational component of the pose.
+ */
+ public Translation2d getTranslation() {
+ return m_translation;
+ }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return The rotational component of the pose.
+ */
+ public Rotation2d getRotation() {
+ return m_rotation;
+ }
+
+ /**
+ * Transforms the pose by the given transformation and returns the new pose.
+ * See + operator for the matrix multiplication performed.
+ *
+ * @param other The transform to transform the pose by.
+ * @return The transformed pose.
+ */
+ public Pose2d transformBy(Transform2d other) {
+ return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+ m_rotation.plus(other.getRotation()));
+ }
+
+ /**
+ * Returns the other pose relative to the current pose.
+ *
+ * <p>This function can often be used for trajectory tracking or pose
+ * stabilization algorithms to get the error between the reference and the
+ * current pose.
+ *
+ * @param other The pose that is the origin of the new coordinate frame that
+ * the current pose will be converted into.
+ * @return The current pose relative to the new origin pose.
+ */
+ public Pose2d relativeTo(Pose2d other) {
+ var transform = new Transform2d(other, this);
+ return new Pose2d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Obtain a new Pose2d from a (constant curvature) velocity.
+ *
+ * <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
+ * Controls Engineering in the FIRST Robotics Competition</a>
+ * section on nonlinear pose estimation for derivation.
+ *
+ * <p>The twist is a change in pose in the robot's coordinate frame since the
+ * previous pose update. When the user runs exp() on the previous known
+ * field-relative pose with the argument being the twist, the user will
+ * receive the new field-relative pose.
+ *
+ * <p>"Exp" represents the pose exponential, which is solving a differential
+ * equation moving the pose forward in time.
+ *
+ * @param twist The change in pose in the robot's coordinate frame since the
+ * previous pose update. For example, if a non-holonomic robot moves forward
+ * 0.01 meters and changes angle by 0.5 degrees since the previous pose update,
+ * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+ * @return The new pose of the robot.
+ */
+ @SuppressWarnings("LocalVariableName")
+ public Pose2d exp(Twist2d twist) {
+ double dx = twist.dx;
+ double dy = twist.dy;
+ double dtheta = twist.dtheta;
+
+ double sinTheta = Math.sin(dtheta);
+ double cosTheta = Math.cos(dtheta);
+
+ double s;
+ double c;
+ if (Math.abs(dtheta) < 1E-9) {
+ s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+ c = 0.5 * dtheta;
+ } else {
+ s = sinTheta / dtheta;
+ c = (1 - cosTheta) / dtheta;
+ }
+ var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
+ new Rotation2d(cosTheta, sinTheta));
+
+ return this.plus(transform);
+ }
+
+ /**
+ * Returns a Twist2d that maps this pose to the end pose. If c is the output
+ * of a.Log(b), then a.Exp(c) would yield b.
+ *
+ * @param end The end pose for the transformation.
+ * @return The twist that maps this to end.
+ */
+ public Twist2d log(Pose2d end) {
+ final var transform = end.relativeTo(this);
+ final var dtheta = transform.getRotation().getRadians();
+ final var halfDtheta = dtheta / 2.0;
+
+ final var cosMinusOne = transform.getRotation().getCos() - 1;
+
+ double halfThetaByTanOfHalfDtheta;
+ if (Math.abs(cosMinusOne) < 1E-9) {
+ halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+ } else {
+ halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
+ }
+
+ Translation2d translationPart = transform.getTranslation().rotateBy(
+ new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
+ ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+ return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+ }
+
+ /**
+ * Checks equality between this Pose2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Pose2d) {
+ return ((Pose2d) obj).m_translation.equals(m_translation)
+ && ((Pose2d) obj).m_rotation.equals(m_rotation);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_translation, m_rotation);
+ }
+
+ static class PoseSerializer extends StdSerializer<Pose2d> {
+ PoseSerializer() {
+ super(Pose2d.class);
+ }
+
+ @Override
+ public void serialize(
+ Pose2d value, JsonGenerator jgen, SerializerProvider provider)
+ throws IOException, JsonProcessingException {
+
+ jgen.writeStartObject();
+ jgen.writeObjectField("translation", value.m_translation);
+ jgen.writeObjectField("rotation", value.m_rotation);
+ jgen.writeEndObject();
+ }
+ }
+
+ static class PoseDeserializer extends StdDeserializer<Pose2d> {
+ PoseDeserializer() {
+ super(Pose2d.class);
+ }
+
+ @Override
+ public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
+ throws IOException, JsonProcessingException {
+ JsonNode node = jp.getCodec().readTree(jp);
+
+ Translation2d translation =
+ jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
+ Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
+ return new Pose2d(translation, rotation);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
new file mode 100644
index 0000000..288fa5c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+@JsonSerialize(using = Rotation2d.RotationSerializer.class)
+@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
+public class Rotation2d {
+ private final double m_value;
+ private final double m_cos;
+ private final double m_sin;
+
+ /**
+ * Constructs a Rotation2d with a default angle of 0 degrees.
+ */
+ public Rotation2d() {
+ m_value = 0.0;
+ m_cos = 1.0;
+ m_sin = 0.0;
+ }
+
+ /**
+ * Constructs a Rotation2d with the given radian value.
+ * The x and y don't have to be normalized.
+ *
+ * @param value The value of the angle in radians.
+ */
+ public Rotation2d(double value) {
+ m_value = value;
+ m_cos = Math.cos(value);
+ m_sin = Math.sin(value);
+ }
+
+ /**
+ * Constructs a Rotation2d with the given x and y (cosine and sine)
+ * components.
+ *
+ * @param x The x component or cosine of the rotation.
+ * @param y The y component or sine of the rotation.
+ */
+ @SuppressWarnings("ParameterName")
+ public Rotation2d(double x, double y) {
+ double magnitude = Math.hypot(x, y);
+ if (magnitude > 1e-6) {
+ m_sin = y / magnitude;
+ m_cos = x / magnitude;
+ } else {
+ m_sin = 0.0;
+ m_cos = 1.0;
+ }
+ m_value = Math.atan2(m_sin, m_cos);
+ }
+
+ /**
+ * Constructs and returns a Rotation2d with the given degree value.
+ *
+ * @param degrees The value of the angle in degrees.
+ * @return The rotation object with the desired angle value.
+ */
+ public static Rotation2d fromDegrees(double degrees) {
+ return new Rotation2d(Math.toRadians(degrees));
+ }
+
+ /**
+ * Adds two rotations together, with the result being bounded between -pi and
+ * pi.
+ *
+ * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
+ * Rotation2d{-pi/2}
+ *
+ * @param other The rotation to add.
+ * @return The sum of the two rotations.
+ */
+ public Rotation2d plus(Rotation2d other) {
+ return rotateBy(other);
+ }
+
+ /**
+ * Subtracts the new rotation from the current rotation and returns the new
+ * rotation.
+ *
+ * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
+ * Rotation2d{-pi/2}
+ *
+ * @param other The rotation to subtract.
+ * @return The difference between the two rotations.
+ */
+ public Rotation2d minus(Rotation2d other) {
+ return rotateBy(other.unaryMinus());
+ }
+
+ /**
+ * Takes the inverse of the current rotation. This is simply the negative of
+ * the current angular value.
+ *
+ * @return The inverse of the current rotation.
+ */
+ public Rotation2d unaryMinus() {
+ return new Rotation2d(-m_value);
+ }
+
+ /**
+ * Multiplies the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Rotation2d.
+ */
+ public Rotation2d times(double scalar) {
+ return new Rotation2d(m_value * scalar);
+ }
+
+ /**
+ * Adds the new rotation to the current rotation using a rotation matrix.
+ *
+ * <p>The matrix multiplication is as follows:
+ * [cos_new] [other.cos, -other.sin][cos]
+ * [sin_new] = [other.sin, other.cos][sin]
+ * value_new = atan2(cos_new, sin_new)
+ *
+ * @param other The rotation to rotate by.
+ * @return The new rotated Rotation2d.
+ */
+ public Rotation2d rotateBy(Rotation2d other) {
+ return new Rotation2d(
+ m_cos * other.m_cos - m_sin * other.m_sin,
+ m_cos * other.m_sin + m_sin * other.m_cos
+ );
+ }
+
+ /*
+ * Returns the radian value of the rotation.
+ *
+ * @return The radian value of the rotation.
+ */
+ public double getRadians() {
+ return m_value;
+ }
+
+ /**
+ * Returns the degree value of the rotation.
+ *
+ * @return The degree value of the rotation.
+ */
+ public double getDegrees() {
+ return Math.toDegrees(m_value);
+ }
+
+ /**
+ * Returns the cosine of the rotation.
+ *
+ * @return The cosine of the rotation.
+ */
+ public double getCos() {
+ return m_cos;
+ }
+
+ /**
+ * Returns the sine of the rotation.
+ *
+ * @return The sine of the rotation.
+ */
+ public double getSin() {
+ return m_sin;
+ }
+
+ /**
+ * Returns the tangent of the rotation.
+ *
+ * @return The tangent of the rotation.
+ */
+ public double getTan() {
+ return m_sin / m_cos;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+ }
+
+ /**
+ * Checks equality between this Rotation2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Rotation2d) {
+ return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_value);
+ }
+
+ static class RotationSerializer extends StdSerializer<Rotation2d> {
+ RotationSerializer() {
+ super(Rotation2d.class);
+ }
+
+ @Override
+ public void serialize(
+ Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
+ throws IOException, JsonProcessingException {
+
+ jgen.writeStartObject();
+ jgen.writeNumberField("radians", value.m_value);
+ jgen.writeEndObject();
+ }
+ }
+
+ static class RotationDeserializer extends StdDeserializer<Rotation2d> {
+ RotationDeserializer() {
+ super(Rotation2d.class);
+ }
+
+ @Override
+ public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
+ throws IOException, JsonProcessingException {
+ JsonNode node = jp.getCodec().readTree(jp);
+ double radians = node.get("radians").numberValue().doubleValue();
+
+ return new Rotation2d(radians);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
new file mode 100644
index 0000000..507ebfe
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+public class Transform2d {
+ private final Translation2d m_translation;
+ private final Rotation2d m_rotation;
+
+ /**
+ * Constructs the transform that maps the initial pose to the final pose.
+ *
+ * @param initial The initial pose for the transformation.
+ * @param last The final pose for the transformation.
+ */
+ public Transform2d(Pose2d initial, Pose2d last) {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ m_translation = last.getTranslation().minus(initial.getTranslation())
+ .rotateBy(initial.getRotation().unaryMinus());
+
+ m_rotation = last.getRotation().minus(initial.getRotation());
+ }
+
+ /**
+ * Constructs a transform with the given translation and rotation components.
+ *
+ * @param translation Translational component of the transform.
+ * @param rotation Rotational component of the transform.
+ */
+ public Transform2d(Translation2d translation, Rotation2d rotation) {
+ m_translation = translation;
+ m_rotation = rotation;
+ }
+
+ /**
+ * Constructs the identity transform -- maps an initial pose to itself.
+ */
+ public Transform2d() {
+ m_translation = new Translation2d();
+ m_rotation = new Rotation2d();
+ }
+
+ /**
+ * Scales the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform2d.
+ */
+ public Transform2d times(double scalar) {
+ return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
+ }
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return The translational component of the transform.
+ */
+ public Translation2d getTranslation() {
+ return m_translation;
+ }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return Reference to the rotational component of the transform.
+ */
+ public Rotation2d getRotation() {
+ return m_rotation;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+ }
+
+ /**
+ * Checks equality between this Transform2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Transform2d) {
+ return ((Transform2d) obj).m_translation.equals(m_translation)
+ && ((Transform2d) obj).m_rotation.equals(m_rotation);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_translation, m_rotation);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
new file mode 100644
index 0000000..73b0257
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
@@ -0,0 +1,239 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * <p>This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+@JsonSerialize(using = Translation2d.TranslationSerializer.class)
+@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
+@SuppressWarnings({"ParameterName", "MemberName"})
+public class Translation2d {
+ private final double m_x;
+ private final double m_y;
+
+ /**
+ * Constructs a Translation2d with X and Y components equal to zero.
+ */
+ public Translation2d() {
+ this(0.0, 0.0);
+ }
+
+ /**
+ * Constructs a Translation2d with the X and Y components equal to the
+ * provided values.
+ *
+ * @param x The x component of the translation.
+ * @param y The y component of the translation.
+ */
+ public Translation2d(double x, double y) {
+ m_x = x;
+ m_y = y;
+ }
+
+ /**
+ * Calculates the distance between two translations in 2d space.
+ *
+ * <p>This function uses the pythagorean theorem to calculate the distance.
+ * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
+ *
+ * @param other The translation to compute the distance to.
+ * @return The distance between the two translations.
+ */
+ public double getDistance(Translation2d other) {
+ return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+ }
+
+ /**
+ * Returns the X component of the translation.
+ *
+ * @return The x component of the translation.
+ */
+ public double getX() {
+ return m_x;
+ }
+
+ /**
+ * Returns the Y component of the translation.
+ *
+ * @return The y component of the translation.
+ */
+ public double getY() {
+ return m_y;
+ }
+
+ /**
+ * Returns the norm, or distance from the origin to the translation.
+ *
+ * @return The norm of the translation.
+ */
+ public double getNorm() {
+ return Math.hypot(m_x, m_y);
+ }
+
+ /**
+ * Applies a rotation to the translation in 2d space.
+ *
+ * <p>This multiplies the translation vector by a counterclockwise rotation
+ * matrix of the given angle.
+ * [x_new] [other.cos, -other.sin][x]
+ * [y_new] = [other.sin, other.cos][y]
+ *
+ * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+ * Translation2d of {0, 2}.
+ *
+ * @param other The rotation to rotate the translation by.
+ * @return The new rotated translation.
+ */
+ public Translation2d rotateBy(Rotation2d other) {
+ return new Translation2d(
+ m_x * other.getCos() - m_y * other.getSin(),
+ m_x * other.getSin() + m_y * other.getCos()
+ );
+ }
+
+ /**
+ * Adds two translations in 2d space and returns the sum. This is similar to
+ * vector addition.
+ *
+ * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+ * Translation2d{3.0, 8.0}
+ *
+ * @param other The translation to add.
+ * @return The sum of the translations.
+ */
+ public Translation2d plus(Translation2d other) {
+ return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+ }
+
+ /**
+ * Subtracts the other translation from the other translation and returns the
+ * difference.
+ *
+ * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+ * Translation2d{4.0, 2.0}
+ *
+ * @param other The translation to subtract.
+ * @return The difference between the two translations.
+ */
+ public Translation2d minus(Translation2d other) {
+ return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+ }
+
+ /**
+ * Returns the inverse of the current translation. This is equivalent to
+ * rotating by 180 degrees, flipping the point over both axes, or simply
+ * negating both components of the translation.
+ *
+ * @return The inverse of the current translation.
+ */
+ public Translation2d unaryMinus() {
+ return new Translation2d(-m_x, -m_y);
+ }
+
+ /**
+ * Multiplies the translation by a scalar and returns the new translation.
+ *
+ * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+ *
+ * @param scalar The scalar to multiply by.
+ * @return The scaled translation.
+ */
+ public Translation2d times(double scalar) {
+ return new Translation2d(m_x * scalar, m_y * scalar);
+ }
+
+ /**
+ * Divides the translation by a scalar and returns the new translation.
+ *
+ * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+ *
+ * @param scalar The scalar to multiply by.
+ * @return The reference to the new mutated object.
+ */
+ public Translation2d div(double scalar) {
+ return new Translation2d(m_x / scalar, m_y / scalar);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+ }
+
+ /**
+ * Checks equality between this Translation2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Translation2d) {
+ return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
+ && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_x, m_y);
+ }
+
+ static class TranslationSerializer extends StdSerializer<Translation2d> {
+ TranslationSerializer() {
+ super(Translation2d.class);
+ }
+
+ @Override
+ public void serialize(
+ Translation2d value, JsonGenerator jgen, SerializerProvider provider)
+ throws IOException, JsonProcessingException {
+
+ jgen.writeStartObject();
+ jgen.writeNumberField("x", value.m_x);
+ jgen.writeNumberField("y", value.m_y);
+ jgen.writeEndObject();
+ }
+ }
+
+ static class TranslationDeserializer extends StdDeserializer<Translation2d> {
+ TranslationDeserializer() {
+ super(Translation2d.class);
+ }
+
+ @Override
+ public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
+ throws IOException, JsonProcessingException {
+ JsonNode node = jp.getCodec().readTree(jp);
+ double xval = node.get("x").numberValue().doubleValue();
+ double yval = node.get("y").numberValue().doubleValue();
+
+ return new Translation2d(xval, yval);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
new file mode 100644
index 0000000..f2def81
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+/**
+ * A change in distance along arc since the last pose update. We can use ideas
+ * from differential calculus to create new Pose2ds from a Twist2d and vise
+ * versa.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+@SuppressWarnings("MemberName")
+public class Twist2d {
+ /**
+ * Linear "dx" component.
+ */
+ public double dx;
+
+ /**
+ * Linear "dy" component.
+ */
+ public double dy;
+
+ /**
+ * Angular "dtheta" component (radians).
+ */
+ public double dtheta;
+
+ public Twist2d() {
+ }
+
+ /**
+ * Constructs a Twist2d with the given values.
+ * @param dx Change in x direction relative to robot.
+ * @param dy Change in y direction relative to robot.
+ * @param dtheta Change in angle relative to robot.
+ */
+ public Twist2d(double dx, double dy, double dtheta) {
+ this.dx = dx;
+ this.dy = dy;
+ this.dtheta = dtheta;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+ }
+
+ /**
+ * Checks equality between this Twist2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Twist2d) {
+ return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
+ && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
+ && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(dx, dy, dtheta);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
new file mode 100644
index 0000000..81086d4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.interfaces;
+
+/**
+ * Interface for 3-axis accelerometers.
+ */
+public interface Accelerometer {
+ enum Range {
+ k2G, k4G, k8G, k16G
+ }
+
+ /**
+ * Common interface for setting the measuring range of an accelerometer.
+ *
+ * @param range The maximum acceleration, positive or negative, that the accelerometer will
+ * measure. Not all accelerometers support all ranges.
+ */
+ void setRange(Range range);
+
+ /**
+ * Common interface for getting the x axis acceleration.
+ *
+ * @return The acceleration along the x axis in g-forces
+ */
+ double getX();
+
+ /**
+ * Common interface for getting the y axis acceleration.
+ *
+ * @return The acceleration along the y axis in g-forces
+ */
+ double getY();
+
+ /**
+ * Common interface for getting the z axis acceleration.
+ *
+ * @return The acceleration along the z axis in g-forces
+ */
+ double getZ();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
new file mode 100644
index 0000000..a21e6d5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.interfaces;
+
+/**
+ * Interface for yaw rate gyros.
+ */
+public interface Gyro extends AutoCloseable {
+ /**
+ * Calibrate the gyro by running for a number of samples and computing the center value. Then use
+ * the center value as the Accumulator center value for subsequent measurements. It's important to
+ * make sure that the robot is not moving while the centering calculations are in progress, this
+ * is typically done when the robot is first turned on while it's sitting at rest before the
+ * competition starts.
+ */
+ void calibrate();
+
+ /**
+ * Reset the gyro. Resets the gyro to a heading of zero. This can be used if there is significant
+ * drift in the gyro and it needs to be recalibrated after it has been running.
+ */
+ void reset();
+
+ /**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * <p>The angle is based on the current accumulator value corrected by the oversampling rate, the
+ * gyro type and the A/D calibration values. The angle is continuous, that is it will continue
+ * from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in
+ * the gyro output as it sweeps past from 360 to 0 on the second time around.
+ *
+ * <p>The angle is expected to increase as the gyro turns clockwise when looked
+ * at from the top. It needs to follow NED axis conventions in order to work
+ * properly with dependent control loops.
+ *
+ * <p>This heading is based on integration of the returned rate from the gyro.
+ *
+ * @return the current heading of the robot in degrees.
+ */
+ double getAngle();
+
+ /**
+ * Return the rate of rotation of the gyro.
+ *
+ * <p>The rate is based on the most recent reading of the gyro analog value
+ *
+ * <p>The rate is expected to be positive as the gyro turns clockwise when looked
+ * at from the top. It needs to follow NED axis conventions in order to work
+ * properly with dependent control loops.
+ *
+ * @return the current rate in degrees per second
+ */
+ double getRate();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
new file mode 100644
index 0000000..ea5c1e6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.interfaces;
+
+import edu.wpi.first.wpilibj.PIDSource;
+
+/**
+ * Interface for a Potentiometer.
+ */
+public interface Potentiometer extends PIDSource {
+ double get();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
new file mode 100644
index 0000000..aa98a47
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
+ * this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of
+ * reference.
+ *
+ * <p>A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+@SuppressWarnings("MemberName")
+public class ChassisSpeeds {
+ /**
+ * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+ */
+ public double vxMetersPerSecond;
+
+ /**
+ * Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
+ */
+ public double vyMetersPerSecond;
+
+ /**
+ * Represents the angular velocity of the robot frame. (CCW is +)
+ */
+ public double omegaRadiansPerSecond;
+
+ /**
+ * Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
+ */
+ public ChassisSpeeds() {
+ }
+
+ /**
+ * Constructs a ChassisSpeeds object.
+ *
+ * @param vxMetersPerSecond Forward velocity.
+ * @param vyMetersPerSecond Sideways velocity.
+ * @param omegaRadiansPerSecond Angular velocity.
+ */
+ public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
+ double omegaRadiansPerSecond) {
+ this.vxMetersPerSecond = vxMetersPerSecond;
+ this.vyMetersPerSecond = vyMetersPerSecond;
+ this.omegaRadiansPerSecond = omegaRadiansPerSecond;
+ }
+
+ /**
+ * Converts a user provided field-relative set of speeds into a robot-relative
+ * ChassisSpeeds object.
+ *
+ * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
+ * Positive x is away from your alliance wall.
+ * @param vyMetersPerSecond The component of speed in the y direction relative to the field.
+ * Positive y is to your left when standing behind the alliance wall.
+ * @param omegaRadiansPerSecond The angular rate of the robot.
+ * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's
+ * angle is considered to be zero when it is facing directly away
+ * from your alliance station wall. Remember that this should
+ * be CCW positive.
+ * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
+ */
+ public static ChassisSpeeds fromFieldRelativeSpeeds(
+ double vxMetersPerSecond, double vyMetersPerSecond,
+ double omegaRadiansPerSecond, Rotation2d robotAngle) {
+ return new ChassisSpeeds(
+ vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
+ -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
+ omegaRadiansPerSecond
+ );
+ }
+
+ @Override
+ public String toString() {
+ return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+ vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
new file mode 100644
index 0000000..bb632af
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * <p>Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveKinematics {
+ public final double trackWidthMeters;
+
+ /**
+ * Constructs a differential drive kinematics object.
+ *
+ * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is
+ * the distance between the left wheels and right wheels.
+ * However, the empirical value may be larger than the physical
+ * measured value due to scrubbing effects.
+ */
+ public DifferentialDriveKinematics(double trackWidthMeters) {
+ this.trackWidthMeters = trackWidthMeters;
+ HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_DifferentialDrive);
+ }
+
+ /**
+ * Returns a chassis speed from left and right component velocities using
+ * forward kinematics.
+ *
+ * @param wheelSpeeds The left and right velocities.
+ * @return The chassis speed.
+ */
+ public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
+ return new ChassisSpeeds(
+ (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
+ (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
+ / trackWidthMeters
+ );
+ }
+
+ /**
+ * Returns left and right component velocities from a chassis speed using
+ * inverse kinematics.
+ *
+ * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+ * represent the chassis' speed.
+ * @return The left and right velocities.
+ */
+ public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+ return new DifferentialDriveWheelSpeeds(
+ chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
+ * chassisSpeeds.omegaRadiansPerSecond,
+ chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2
+ * chassisSpeeds.omegaRadiansPerSecond
+ );
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
new file mode 100644
index 0000000..35a4342
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * <p>It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
+ */
+public class DifferentialDriveOdometry {
+ private Pose2d m_poseMeters;
+
+ private Rotation2d m_gyroOffset;
+ private Rotation2d m_previousAngle;
+
+ private double m_prevLeftDistance;
+ private double m_prevRightDistance;
+
+ /**
+ * Constructs a DifferentialDriveOdometry object.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param initialPoseMeters The starting position of the robot on the field.
+ */
+ public DifferentialDriveOdometry(Rotation2d gyroAngle,
+ Pose2d initialPoseMeters) {
+ m_poseMeters = initialPoseMeters;
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousAngle = initialPoseMeters.getRotation();
+ HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
+ }
+
+ /**
+ * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ */
+ public DifferentialDriveOdometry(Rotation2d gyroAngle) {
+ this(gyroAngle, new Pose2d());
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * <p>You NEED to reset your encoders (to zero) when calling this method.
+ *
+ * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+ * The library automatically takes care of offsetting the gyro angle.
+ *
+ * @param poseMeters The position on the field that your robot is at.
+ * @param gyroAngle The angle reported by the gyroscope.
+ */
+ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ m_poseMeters = poseMeters;
+ m_previousAngle = poseMeters.getRotation();
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+
+ m_prevLeftDistance = 0.0;
+ m_prevRightDistance = 0.0;
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ *
+ * @return The pose of the robot (x and y are in meters).
+ */
+ public Pose2d getPoseMeters() {
+ return m_poseMeters;
+ }
+
+
+ /**
+ * Updates the robot position on the field using distance measurements from encoders. This
+ * method is more numerically accurate than using velocities to integrate the pose and
+ * is also advantageous for teams that are using lower CPR encoders.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
+ * @return The new pose of the robot.
+ */
+ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
+ double rightDistanceMeters) {
+ double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+ double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
+
+ m_prevLeftDistance = leftDistanceMeters;
+ m_prevRightDistance = rightDistanceMeters;
+
+ double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+ var angle = gyroAngle.plus(m_gyroOffset);
+
+ var newPose = m_poseMeters.exp(
+ new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
+
+ m_previousAngle = angle;
+
+ m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+ return m_poseMeters;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
new file mode 100644
index 0000000..ac4a9fa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+/**
+ * Represents the wheel speeds for a differential drive drivetrain.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveWheelSpeeds {
+ /**
+ * Speed of the left side of the robot.
+ */
+ public double leftMetersPerSecond;
+
+ /**
+ * Speed of the right side of the robot.
+ */
+ public double rightMetersPerSecond;
+
+ /**
+ * Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
+ */
+ public DifferentialDriveWheelSpeeds() {
+ }
+
+ /**
+ * Constructs a DifferentialDriveWheelSpeeds.
+ *
+ * @param leftMetersPerSecond The left speed.
+ * @param rightMetersPerSecond The right speed.
+ */
+ public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
+ this.leftMetersPerSecond = leftMetersPerSecond;
+ this.rightMetersPerSecond = rightMetersPerSecond;
+ }
+
+ /**
+ * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+ * after inverse kinematics, the requested speed from a/several modules may be
+ * above the max attainable speed for the driving motor on that module. To fix
+ * this issue, one can "normalize" all the wheel speeds to make sure that all
+ * requested module speeds are below the absolute threshold, while maintaining
+ * the ratio of speeds between modules.
+ *
+ * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+ */
+ public void normalize(double attainableMaxSpeedMetersPerSecond) {
+ double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
+
+ if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+ leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+ leftMetersPerSecond, rightMetersPerSecond);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
new file mode 100644
index 0000000..a5e5b5f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
@@ -0,0 +1,173 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds.
+ *
+ * <p>The inverse kinematics (converting from a desired chassis velocity to
+ * individual wheel speeds) uses the relative locations of the wheels with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion manuevers.
+ *
+ * <p>Forward kinematics (converting an array of wheel speeds into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
+ * multiply by [wheelSpeeds] to get our chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+public class MecanumDriveKinematics {
+ private SimpleMatrix m_inverseKinematics;
+ private final SimpleMatrix m_forwardKinematics;
+
+ private final Translation2d m_frontLeftWheelMeters;
+ private final Translation2d m_frontRightWheelMeters;
+ private final Translation2d m_rearLeftWheelMeters;
+ private final Translation2d m_rearRightWheelMeters;
+
+ private Translation2d m_prevCoR = new Translation2d();
+
+ /**
+ * Constructs a mecanum drive kinematics object.
+ *
+ * @param frontLeftWheelMeters The location of the front-left wheel relative to the
+ * physical center of the robot.
+ * @param frontRightWheelMeters The location of the front-right wheel relative to
+ * the physical center of the robot.
+ * @param rearLeftWheelMeters The location of the rear-left wheel relative to the
+ * physical center of the robot.
+ * @param rearRightWheelMeters The location of the rear-right wheel relative to the
+ * physical center of the robot.
+ */
+ public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
+ Translation2d frontRightWheelMeters,
+ Translation2d rearLeftWheelMeters,
+ Translation2d rearRightWheelMeters) {
+ m_frontLeftWheelMeters = frontLeftWheelMeters;
+ m_frontRightWheelMeters = frontRightWheelMeters;
+ m_rearLeftWheelMeters = rearLeftWheelMeters;
+ m_rearRightWheelMeters = rearRightWheelMeters;
+
+ m_inverseKinematics = new SimpleMatrix(4, 3);
+
+ setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
+ rearLeftWheelMeters, rearRightWheelMeters);
+ m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+ HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
+ }
+
+ /**
+ * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+ * method is often used to convert joystick values into wheel speeds.
+ *
+ * <p>This function also supports variable centers of rotation. During normal
+ * operations, the center of rotation is usually the same as the physical
+ * center of the robot; therefore, the argument is defaulted to that use case.
+ * However, if you wish to change the center of rotation for evasive
+ * manuevers, vision alignment, or for any other use case, you can do so.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @param centerOfRotationMeters The center of rotation. For example, if you set the
+ * center of rotation at one corner of the robot and provide
+ * a chassis speed that only has a dtheta component, the robot
+ * will rotate around that corner.
+ * @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user
+ * input may cause one of the wheel speeds to go above the attainable max velocity. Use
+ * the {@link MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
+ */
+ public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
+ Translation2d centerOfRotationMeters) {
+ // We have a new center of rotation. We need to compute the matrix again.
+ if (!centerOfRotationMeters.equals(m_prevCoR)) {
+ var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
+ var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
+ var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
+ var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
+
+ setInverseKinematics(fl, fr, rl, rr);
+ m_prevCoR = centerOfRotationMeters;
+ }
+
+ var chassisSpeedsVector = new SimpleMatrix(3, 1);
+ chassisSpeedsVector.setColumn(0, 0,
+ chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
+ chassisSpeeds.omegaRadiansPerSecond);
+
+ var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+ return new MecanumDriveWheelSpeeds(
+ wheelsMatrix.get(0, 0),
+ wheelsMatrix.get(1, 0),
+ wheelsMatrix.get(2, 0),
+ wheelsMatrix.get(3, 0)
+ );
+ }
+
+ /**
+ * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
+ * information.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @return The wheel speeds.
+ */
+ public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+ return toWheelSpeeds(chassisSpeeds, new Translation2d());
+ }
+
+ /**
+ * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
+ * This method is often used for odometry -- determining the robot's position on the field using
+ * data from the real-world speed of each wheel on the robot.
+ *
+ * @param wheelSpeeds The current mecanum drive wheel speeds.
+ * @return The resulting chassis speed.
+ */
+ public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+ var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
+ wheelSpeedsMatrix.setColumn(0, 0,
+ wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
+ wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
+ );
+ var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
+
+ return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
+ chassisSpeedsVector.get(2, 0));
+ }
+
+ /**
+ * Construct inverse kinematics matrix from wheel locations.
+ *
+ * @param fl The location of the front-left wheel relative to the physical center of the robot.
+ * @param fr The location of the front-right wheel relative to the physical center of the robot.
+ * @param rl The location of the rear-left wheel relative to the physical center of the robot.
+ * @param rr The location of the rear-right wheel relative to the physical center of the robot.
+ */
+ private void setInverseKinematics(Translation2d fl, Translation2d fr,
+ Translation2d rl, Translation2d rr) {
+ m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
+ m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
+ m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
+ m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
+ m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
new file mode 100644
index 0000000..f467d20
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+/**
+ * Represents the motor voltages for a mecanum drive drivetrain.
+ */
+@SuppressWarnings("MemberName")
+public class MecanumDriveMotorVoltages {
+ /**
+ * Voltage of the front left motor.
+ */
+ public double frontLeftVoltage;
+
+ /**
+ * Voltage of the front right motor.
+ */
+ public double frontRightVoltage;
+
+ /**
+ * Voltage of the rear left motor.
+ */
+ public double rearLeftVoltage;
+
+ /**
+ * Voltage of the rear right motor.
+ */
+ public double rearRightVoltage;
+
+ /**
+ * Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
+ */
+ public MecanumDriveMotorVoltages() {
+ }
+
+ /**
+ * Constructs a MecanumDriveMotorVoltages.
+ *
+ * @param frontLeftVoltage Voltage of the front left motor.
+ * @param frontRightVoltage Voltage of the front right motor.
+ * @param rearLeftVoltage Voltage of the rear left motor.
+ * @param rearRightVoltage Voltage of the rear right motor.
+ */
+ public MecanumDriveMotorVoltages(double frontLeftVoltage,
+ double frontRightVoltage,
+ double rearLeftVoltage,
+ double rearRightVoltage) {
+ this.frontLeftVoltage = frontLeftVoltage;
+ this.frontRightVoltage = frontRightVoltage;
+ this.rearLeftVoltage = rearLeftVoltage;
+ this.rearRightVoltage = rearRightVoltage;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
+ rearLeftVoltage, rearRightVoltage);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
new file mode 100644
index 0000000..b3dd402
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * mecanum wheel encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+public class MecanumDriveOdometry {
+ private final MecanumDriveKinematics m_kinematics;
+ private Pose2d m_poseMeters;
+ private double m_prevTimeSeconds = -1;
+
+ private Rotation2d m_gyroOffset;
+ private Rotation2d m_previousAngle;
+
+ /**
+ * Constructs a MecanumDriveOdometry object.
+ *
+ * @param kinematics The mecanum drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param initialPoseMeters The starting position of the robot on the field.
+ */
+ public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
+ Pose2d initialPoseMeters) {
+ m_kinematics = kinematics;
+ m_poseMeters = initialPoseMeters;
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousAngle = initialPoseMeters.getRotation();
+ HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
+ }
+
+ /**
+ * Constructs a MecanumDriveOdometry object with the default pose at the origin.
+ *
+ * @param kinematics The mecanum drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ */
+ public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
+ this(kinematics, gyroAngle, new Pose2d());
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+ * The library automatically takes care of offsetting the gyro angle.
+ *
+ * @param poseMeters The position on the field that your robot is at.
+ * @param gyroAngle The angle reported by the gyroscope.
+ */
+ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ m_poseMeters = poseMeters;
+ m_previousAngle = poseMeters.getRotation();
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ *
+ * @return The pose of the robot (x and y are in meters).
+ */
+ public Pose2d getPoseMeters() {
+ return m_poseMeters;
+ }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method takes in the current time as
+ * a parameter to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param currentTimeSeconds The current time in seconds.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelSpeeds The current wheel speeds.
+ * @return The new pose of the robot.
+ */
+ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+ MecanumDriveWheelSpeeds wheelSpeeds) {
+ double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+ m_prevTimeSeconds = currentTimeSeconds;
+
+ var angle = gyroAngle.plus(m_gyroOffset);
+
+ var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
+ var newPose = m_poseMeters.exp(
+ new Twist2d(chassisState.vxMetersPerSecond * period,
+ chassisState.vyMetersPerSecond * period,
+ angle.minus(m_previousAngle).getRadians()));
+
+ m_previousAngle = angle;
+ m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+ return m_poseMeters;
+ }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method automatically calculates the
+ * current time to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelSpeeds The current wheel speeds.
+ * @return The new pose of the robot.
+ */
+ public Pose2d update(Rotation2d gyroAngle,
+ MecanumDriveWheelSpeeds wheelSpeeds) {
+ return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
+ wheelSpeeds);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
new file mode 100644
index 0000000..0f0dd71
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import java.util.stream.DoubleStream;
+
+@SuppressWarnings("MemberName")
+public class MecanumDriveWheelSpeeds {
+ /**
+ * Speed of the front left wheel.
+ */
+ public double frontLeftMetersPerSecond;
+
+ /**
+ * Speed of the front right wheel.
+ */
+ public double frontRightMetersPerSecond;
+
+ /**
+ * Speed of the rear left wheel.
+ */
+ public double rearLeftMetersPerSecond;
+
+ /**
+ * Speed of the rear right wheel.
+ */
+ public double rearRightMetersPerSecond;
+
+ /**
+ * Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
+ */
+ public MecanumDriveWheelSpeeds() {
+ }
+
+ /**
+ * Constructs a MecanumDriveWheelSpeeds.
+ *
+ * @param frontLeftMetersPerSecond Speed of the front left wheel.
+ * @param frontRightMetersPerSecond Speed of the front right wheel.
+ * @param rearLeftMetersPerSecond Speed of the rear left wheel.
+ * @param rearRightMetersPerSecond Speed of the rear right wheel.
+ */
+ public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
+ double frontRightMetersPerSecond,
+ double rearLeftMetersPerSecond,
+ double rearRightMetersPerSecond) {
+ this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
+ this.frontRightMetersPerSecond = frontRightMetersPerSecond;
+ this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
+ this.rearRightMetersPerSecond = rearRightMetersPerSecond;
+ }
+
+ /**
+ * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+ * after inverse kinematics, the requested speed from a/several modules may be
+ * above the max attainable speed for the driving motor on that module. To fix
+ * this issue, one can "normalize" all the wheel speeds to make sure that all
+ * requested module speeds are below the absolute threshold, while maintaining
+ * the ratio of speeds between modules.
+ *
+ * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+ */
+ public void normalize(double attainableMaxSpeedMetersPerSecond) {
+ double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
+ frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
+ .max().getAsDouble();
+
+ if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+ frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
+ frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
new file mode 100644
index 0000000..37f97b8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
@@ -0,0 +1,200 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import java.util.Arrays;
+import java.util.Collections;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual module states (speed and angle).
+ *
+ * <p>The inverse kinematics (converting from a desired chassis velocity to
+ * individual module states) uses the relative locations of the modules with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion manuevers.
+ *
+ * <p>Forward kinematics (converting an array of module states into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
+ * multiply by [moduleStates] to get our chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+public class SwerveDriveKinematics {
+ private final SimpleMatrix m_inverseKinematics;
+ private final SimpleMatrix m_forwardKinematics;
+
+ private final int m_numModules;
+ private final Translation2d[] m_modules;
+ private Translation2d m_prevCoR = new Translation2d();
+
+ /**
+ * Constructs a swerve drive kinematics object. This takes in a variable
+ * number of wheel locations as Translation2ds. The order in which you pass in
+ * the wheel locations is the same order that you will recieve the module
+ * states when performing inverse kinematics. It is also expected that you
+ * pass in the module states in the same order when calling the forward
+ * kinematics methods.
+ *
+ * @param wheelsMeters The locations of the wheels relative to the physical center
+ * of the robot.
+ */
+ public SwerveDriveKinematics(Translation2d... wheelsMeters) {
+ if (wheelsMeters.length < 2) {
+ throw new IllegalArgumentException("A swerve drive requires at least two modules");
+ }
+ m_numModules = wheelsMeters.length;
+ m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+ m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+
+ for (int i = 0; i < m_numModules; i++) {
+ m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
+ m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
+ }
+ m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+ HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
+ }
+
+ /**
+ * Performs inverse kinematics to return the module states from a desired
+ * chassis velocity. This method is often used to convert joystick values into
+ * module speeds and angles.
+ *
+ * <p>This function also supports variable centers of rotation. During normal
+ * operations, the center of rotation is usually the same as the physical
+ * center of the robot; therefore, the argument is defaulted to that use case.
+ * However, if you wish to change the center of rotation for evasive
+ * manuevers, vision alignment, or for any other use case, you can do so.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @param centerOfRotationMeters The center of rotation. For example, if you set the
+ * center of rotation at one corner of the robot and provide
+ * a chassis speed that only has a dtheta component, the robot
+ * will rotate around that corner.
+ * @return An array containing the module states. Use caution because these
+ * module states are not normalized. Sometimes, a user input may cause one of
+ * the module speeds to go above the attainable max velocity. Use the
+ * {@link #normalizeWheelSpeeds(SwerveModuleState[], double) normalizeWheelSpeeds}
+ * function to rectify this issue.
+ */
+ @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+ public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
+ Translation2d centerOfRotationMeters) {
+ if (!centerOfRotationMeters.equals(m_prevCoR)) {
+ for (int i = 0; i < m_numModules; i++) {
+ m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
+ -m_modules[i].getY() + centerOfRotationMeters.getY());
+ m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
+ +m_modules[i].getX() - centerOfRotationMeters.getX());
+ }
+ m_prevCoR = centerOfRotationMeters;
+ }
+
+ var chassisSpeedsVector = new SimpleMatrix(3, 1);
+ chassisSpeedsVector.setColumn(0, 0,
+ chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
+ chassisSpeeds.omegaRadiansPerSecond);
+
+ var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+ SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
+
+ for (int i = 0; i < m_numModules; i++) {
+ double x = moduleStatesMatrix.get(i * 2, 0);
+ double y = moduleStatesMatrix.get(i * 2 + 1, 0);
+
+ double speed = Math.hypot(x, y);
+ Rotation2d angle = new Rotation2d(x, y);
+
+ moduleStates[i] = new SwerveModuleState(speed, angle);
+ }
+
+ return moduleStates;
+ }
+
+ /**
+ * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
+ * toSwerveModuleStates for more information.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @return An array containing the module states.
+ */
+ public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
+ return toSwerveModuleStates(chassisSpeeds, new Translation2d());
+ }
+
+ /**
+ * Performs forward kinematics to return the resulting chassis state from the
+ * given module states. This method is often used for odometry -- determining
+ * the robot's position on the field using data from the real-world speed and
+ * angle of each module on the robot.
+ *
+ * @param wheelStates The state of the modules (as a SwerveModuleState type)
+ * as measured from respective encoders and gyros. The order of the swerve
+ * module states should be same as passed into the constructor of this class.
+ * @return The resulting chassis speed.
+ */
+ public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
+ if (wheelStates.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor"
+ );
+ }
+ var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+ for (int i = 0; i < m_numModules; i++) {
+ var module = wheelStates[i];
+ moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
+ moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
+ }
+
+ var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
+ return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
+ chassisSpeedsVector.get(2, 0));
+
+ }
+
+ /**
+ * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+ * after inverse kinematics, the requested speed from a/several modules may be
+ * above the max attainable speed for the driving motor on that module. To fix
+ * this issue, one can "normalize" all the wheel speeds to make sure that all
+ * requested module speeds are below the absolute threshold, while maintaining
+ * the ratio of speeds between modules.
+ *
+ * @param moduleStates Reference to array of module states. The array will be
+ * mutated with the normalized speeds!
+ * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
+ */
+ public static void normalizeWheelSpeeds(SwerveModuleState[] moduleStates,
+ double attainableMaxSpeedMetersPerSecond) {
+ double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+ if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+ for (SwerveModuleState moduleState : moduleStates) {
+ moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
+ * attainableMaxSpeedMetersPerSecond;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
new file mode 100644
index 0000000..811c3de
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+public class SwerveDriveOdometry {
+ private final SwerveDriveKinematics m_kinematics;
+ private Pose2d m_poseMeters;
+ private double m_prevTimeSeconds = -1;
+
+ private Rotation2d m_gyroOffset;
+ private Rotation2d m_previousAngle;
+
+ /**
+ * Constructs a SwerveDriveOdometry object.
+ *
+ * @param kinematics The swerve drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param initialPose The starting position of the robot on the field.
+ */
+ public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
+ Pose2d initialPose) {
+ m_kinematics = kinematics;
+ m_poseMeters = initialPose;
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousAngle = initialPose.getRotation();
+ HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
+ }
+
+ /**
+ * Constructs a SwerveDriveOdometry object with the default pose at the origin.
+ *
+ * @param kinematics The swerve drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ */
+ public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+ this(kinematics, gyroAngle, new Pose2d());
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+ * The library automatically takes care of offsetting the gyro angle.
+ *
+ * @param pose The position on the field that your robot is at.
+ * @param gyroAngle The angle reported by the gyroscope.
+ */
+ public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+ m_poseMeters = pose;
+ m_previousAngle = pose.getRotation();
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ *
+ * @return The pose of the robot (x and y are in meters).
+ */
+ public Pose2d getPoseMeters() {
+ return m_poseMeters;
+ }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method takes in the current time as
+ * a parameter to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param currentTimeSeconds The current time in seconds.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param moduleStates The current state of all swerve modules. Please provide
+ * the states in the same order in which you instantiated your
+ * SwerveDriveKinematics.
+ * @return The new pose of the robot.
+ */
+ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+ SwerveModuleState... moduleStates) {
+ double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+ m_prevTimeSeconds = currentTimeSeconds;
+
+ var angle = gyroAngle.plus(m_gyroOffset);
+
+ var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
+ var newPose = m_poseMeters.exp(
+ new Twist2d(chassisState.vxMetersPerSecond * period,
+ chassisState.vyMetersPerSecond * period,
+ angle.minus(m_previousAngle).getRadians()));
+
+ m_previousAngle = angle;
+ m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+ return m_poseMeters;
+ }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method automatically calculates the
+ * current time to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the angular
+ * rate that is calculated from forward kinematics.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param moduleStates The current state of all swerve modules. Please provide
+ * the states in the same order in which you instantiated your
+ * SwerveDriveKinematics.
+ * @return The new pose of the robot.
+ */
+ public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+ return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
new file mode 100644
index 0000000..e3119e5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents the state of one swerve module.
+ */
+@SuppressWarnings("MemberName")
+public class SwerveModuleState implements Comparable<SwerveModuleState> {
+
+ /**
+ * Speed of the wheel of the module.
+ */
+ public double speedMetersPerSecond;
+
+ /**
+ * Angle of the module.
+ */
+ public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+ /**
+ * Constructs a SwerveModuleState with zeros for speed and angle.
+ */
+ public SwerveModuleState() {
+ }
+
+ /**
+ * Constructs a SwerveModuleState.
+ *
+ * @param speedMetersPerSecond The speed of the wheel of the module.
+ * @param angle The angle of the module.
+ */
+ public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
+ this.speedMetersPerSecond = speedMetersPerSecond;
+ this.angle = angle;
+ }
+
+ /**
+ * Compares two swerve module states. One swerve module is "greater" than the other if its speed
+ * is higher than the other.
+ *
+ * @param o The other swerve module.
+ * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+ */
+ @Override
+ @SuppressWarnings("ParameterName")
+ public int compareTo(SwerveModuleState o) {
+ return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
+ angle);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
new file mode 100644
index 0000000..5a4fb36
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.livewindow;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+
+/**
+ * The LiveWindow class is the public interface for putting sensors and actuators on the
+ * LiveWindow.
+ */
+public class LiveWindow {
+ private static class Component {
+ boolean m_firstTime = true;
+ boolean m_telemetryEnabled = true;
+ }
+
+ private static final int dataHandle = SendableRegistry.getDataHandle();
+ private static final NetworkTable liveWindowTable =
+ NetworkTableInstance.getDefault().getTable("LiveWindow");
+ private static final NetworkTable statusTable = liveWindowTable.getSubTable(".status");
+ private static final NetworkTableEntry enabledEntry = statusTable.getEntry("LW Enabled");
+ private static boolean startLiveWindow;
+ private static boolean liveWindowEnabled;
+ private static boolean telemetryEnabled = true;
+
+ private static Component getOrAdd(Sendable sendable) {
+ Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
+ if (data == null) {
+ data = new Component();
+ SendableRegistry.setData(sendable, dataHandle, data);
+ }
+ return data;
+ }
+
+ private LiveWindow() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ public static synchronized boolean isEnabled() {
+ return liveWindowEnabled;
+ }
+
+ /**
+ * Set the enabled state of LiveWindow. If it's being enabled, turn off the scheduler and remove
+ * all the commands from the queue and enable all the components registered for LiveWindow. If
+ * it's being disabled, stop all the registered components and reenable the scheduler. TODO: add
+ * code to disable PID loops when enabling LiveWindow. The commands should reenable the PID loops
+ * themselves when they get rescheduled. This prevents arms from starting to move around, etc.
+ * after a period of adjusting them in LiveWindow mode.
+ */
+ public static synchronized void setEnabled(boolean enabled) {
+ if (liveWindowEnabled != enabled) {
+ startLiveWindow = enabled;
+ liveWindowEnabled = enabled;
+ updateValues(); // Force table generation now to make sure everything is defined
+ if (enabled) {
+ System.out.println("Starting live window mode.");
+ } else {
+ System.out.println("stopping live window mode.");
+ SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
+ cbdata.builder.stopLiveWindowMode();
+ });
+ }
+ enabledEntry.setBoolean(enabled);
+ }
+ }
+
+ /**
+ * Enable telemetry for a single component.
+ *
+ * @param sendable component
+ */
+ public static synchronized void enableTelemetry(Sendable sendable) {
+ // Re-enable global setting in case disableAllTelemetry() was called.
+ telemetryEnabled = true;
+ getOrAdd(sendable).m_telemetryEnabled = true;
+ }
+
+ /**
+ * Disable telemetry for a single component.
+ *
+ * @param sendable component
+ */
+ public static synchronized void disableTelemetry(Sendable sendable) {
+ getOrAdd(sendable).m_telemetryEnabled = false;
+ }
+
+ /**
+ * Disable ALL telemetry.
+ */
+ public static synchronized void disableAllTelemetry() {
+ telemetryEnabled = false;
+ SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
+ if (cbdata.data == null) {
+ cbdata.data = new Component();
+ }
+ ((Component) cbdata.data).m_telemetryEnabled = false;
+ });
+ }
+
+ /**
+ * Tell all the sensors to update (send) their values.
+ *
+ * <p>Actuators are handled through callbacks on their value changing from the
+ * SmartDashboard widgets.
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ public static synchronized void updateValues() {
+ // Only do this if either LiveWindow mode or telemetry is enabled.
+ if (!liveWindowEnabled && !telemetryEnabled) {
+ return;
+ }
+
+ SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
+ if (cbdata.sendable == null || cbdata.parent != null) {
+ return;
+ }
+
+ if (cbdata.data == null) {
+ cbdata.data = new Component();
+ }
+
+ Component component = (Component) cbdata.data;
+
+ if (!liveWindowEnabled && !component.m_telemetryEnabled) {
+ return;
+ }
+
+ if (component.m_firstTime) {
+ // By holding off creating the NetworkTable entries, it allows the
+ // components to be redefined. This allows default sensor and actuator
+ // values to be created that are replaced with the custom names from
+ // users calling setName.
+ if (cbdata.name.isEmpty()) {
+ return;
+ }
+ NetworkTable ssTable = liveWindowTable.getSubTable(cbdata.subsystem);
+ NetworkTable table;
+ // Treat name==subsystem as top level of subsystem
+ if (cbdata.name.equals(cbdata.subsystem)) {
+ table = ssTable;
+ } else {
+ table = ssTable.getSubTable(cbdata.name);
+ }
+ table.getEntry(".name").setString(cbdata.name);
+ cbdata.builder.setTable(table);
+ cbdata.sendable.initSendable(cbdata.builder);
+ ssTable.getEntry(".type").setString("LW Subsystem");
+
+ component.m_firstTime = false;
+ }
+
+ if (startLiveWindow) {
+ cbdata.builder.startLiveWindowMode();
+ }
+ cbdata.builder.updateTable();
+ });
+
+ startLiveWindow = false;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/package.html b/wpilibj/src/main/java/edu/wpi/first/wpilibj/package.html
new file mode 100644
index 0000000..d11ae27
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/package.html
@@ -0,0 +1,30 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html>
+ <head>
+ <title>
+ WPI Robotics library
+ </title>
+ <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+ </head>
+ <body>
+ The WPI Robotics library (WPILibJ) is a set of Java classes that interfaces
+ to the hardware in the FRC control system and your robot. There are classes
+ to handle sensors, motors, the driver station, and a number of other
+ utility functions like timing and field management. The library is designed
+ to:
+ <ul>
+ <li>Deal with all the low level interfacing to these components so you
+ can concentrate on solving this year's "robot problem". This is a
+ philosophical decision to let you focus on the higher-level design of
+ your robot rather than deal with the details of the processor and the
+ operating system.
+ </li>
+ <li>Understand everything at all levels by making the full source code of
+ the library available. You can study (and modify) the algorithms used by
+ the gyro class for oversampling and integration of the input signal or
+ just ask the class for the current robot heading. You can work at any
+ level.
+ </li>
+ </ul>
+ </body>
+</html>
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
new file mode 100644
index 0000000..27c3e8c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * The types of layouts bundled with Shuffleboard.
+ *
+ * <pre>{@code
+ * ShuffleboardLayout myList = Shuffleboard.getTab("My Tab")
+ * .getLayout(BuiltinLayouts.kList, "My List");
+ * }</pre>
+ */
+public enum BuiltInLayouts implements LayoutType {
+ /**
+ * Groups components in a vertical list. New widgets added to the layout will be placed at the
+ * bottom of the list.
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Label position</td><td>String</td><td>"BOTTOM"</td>
+ * <td>The position of component labels inside the grid. One of
+ * {@code ["TOP", "LEFT", "BOTTOM", "RIGHT", "HIDDEN"}</td></tr>
+ * </table>
+ */
+ kList("List Layout"),
+
+ /**
+ * Groups components in an <i>n</i> x <i>m</i> grid. Grid layouts default to 3x3.
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Number of columns</td><td>Number</td><td>3</td><td>Must be in the range [1,15]</td>
+ * </tr>
+ * <tr><td>Number of rows</td><td>Number</td><td>3</td><td>Must be in the range [1,15]</td></tr>
+ * <tr>
+ * <td>Label position</td>
+ * <td>String</td>
+ * <td>"BOTTOM"</td>
+ * <td>The position of component labels inside the grid.
+ * One of {@code ["TOP", "LEFT", "BOTTOM", "RIGHT", "HIDDEN"}</td>
+ * </tr>
+ * </table>
+ */
+ kGrid("Grid Layout"),
+ ;
+
+ private final String m_layoutName;
+
+ BuiltInLayouts(String layoutName) {
+ m_layoutName = layoutName;
+ }
+
+ @Override
+ public String getLayoutName() {
+ return m_layoutName;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
new file mode 100644
index 0000000..14742e5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
@@ -0,0 +1,399 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.wpilibj.interfaces.Accelerometer.Range;
+
+/**
+ * The types of the widgets bundled with Shuffleboard.
+ *
+ * <p>For example, setting a number to be displayed with a slider:
+ * <pre>{@code
+ * NetworkTableEntry example = Shuffleboard.getTab("My Tab")
+ * .add("My Number", 0)
+ * .withWidget(BuiltInWidgets.kNumberSlider)
+ * .withProperties(Map.of("min", 0, "max", 1))
+ * .getEntry();
+ * }</pre>
+ *
+ * <p>Each value in this enum goes into detail on what data types that widget can support, as well
+ * as the custom properties that widget uses.
+ */
+public enum BuiltInWidgets implements WidgetType {
+ /**
+ * Displays a value with a simple text field.
+ * <br>Supported types:
+ * <ul>
+ * <li>String</li>
+ * <li>Number</li>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kTextView("Text View"),
+ /**
+ * Displays a number with a controllable slider.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>-1.0</td><td>The minimum value of the slider</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>1.0</td><td>The maximum value of the slider</td></tr>
+ * <tr><td>Block increment</td><td>Number</td><td>0.0625</td>
+ * <td>How much to move the slider by with the arrow keys</td></tr>
+ * </table>
+ */
+ kNumberSlider("Number Slider"),
+ /**
+ * Displays a number with a view-only bar.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>-1.0</td><td>The minimum value of the bar</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>1.0</td><td>The maximum value of the bar</td></tr>
+ * <tr><td>Center</td><td>Number</td><td>0</td><td>The center ("zero") value of the bar</td></tr>
+ * </table>
+ */
+ kNumberBar("Number Bar"),
+ /**
+ * Displays a number with a view-only dial. Displayed values are rounded to the nearest integer.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>0</td><td>The minimum value of the dial</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>100</td><td>The maximum value of the dial</td></tr>
+ * <tr><td>Show value</td><td>Boolean</td><td>true</td>
+ * <td>Whether or not to show the value as text</td></tr>
+ * </table>
+ */
+ kDial("Simple Dial"),
+ /**
+ * Displays a number with a graph. <strong>NOTE:</strong> graphs can be taxing on the computer
+ * running the dashboard. Keep the number of visible data points to a minimum. Making the widget
+ * smaller also helps with performance, but may cause the graph to become difficult to read.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * <li>Number array</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Visible time</td><td>Number</td><td>30</td>
+ * <td>How long, in seconds, should past data be visible for</td></tr>
+ * </table>
+ */
+ kGraph("Graph"),
+ /**
+ * Displays a boolean value as a large colored box.
+ * <br>Supported types:
+ * <ul>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Color when true</td><td>Color</td><td>"green"</td>
+ * <td>Can be specified as a string ({@code "#00FF00"}) or a rgba integer ({@code 0x00FF0000})
+ * </td></tr>
+ * <tr><td>Color when false</td><td>Color</td><td>"red"</td>
+ * <td>Can be specified as a string or a number</td></tr>
+ * </table>
+ */
+ kBooleanBox("Boolean Box"),
+ /**
+ * Displays a boolean with a large interactive toggle button.
+ * <br>Supported types:
+ * <ul>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kToggleButton("Toggle Button"),
+ /**
+ * Displays a boolean with a fixed-size toggle switch.
+ * <br>Supported types:
+ * <ul>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kToggleSwitch("Toggle Switch"),
+ /**
+ * Displays an analog input or a raw number with a number bar.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * <li>{@link edu.wpi.first.wpilibj.AnalogInput}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>0</td><td>The minimum value of the bar</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>5</td><td>The maximum value of the bar</td></tr>
+ * <tr><td>Center</td><td>Number</td><td>0</td><td>The center ("zero") value of the bar</td></tr>
+ * <tr><td>Orientation</td><td>String</td><td>"HORIZONTAL"</td>
+ * <td>The orientation of the bar. One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
+ * <tr><td>Number of tick marks</td><td>Number</td><td>5</td>
+ * <td>The number of discrete ticks on the bar</td></tr>
+ * </table>
+ */
+ kVoltageView("Voltage View"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.PowerDistributionPanel PowerDistributionPanel}.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.PowerDistributionPanel}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Show voltage and current values</td><td>Boolean</td><td>true</td>
+ * <td>Whether or not to display the voltage and current draw</td></tr>
+ * </table>
+ */
+ kPowerDistributionPanel("PDP"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with
+ * a dropdown combo box with a list of options.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kComboBoxChooser("ComboBox Chooser"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with
+ * a toggle button for each available option.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kSplitButtonChooser("Split Button Chooser"),
+ /**
+ * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed, total travelled
+ * distance, and its distance per tick.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.Encoder}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kEncoder("Encoder"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.SpeedController SpeedController}. The speed controller
+ * will be controllable from the dashboard when test mode is enabled, but will otherwise be
+ * view-only.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.PWMSpeedController}</li>
+ * <li>{@link edu.wpi.first.wpilibj.DMC60}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Jaguar}</li>
+ * <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}</li>
+ * <li>{@link edu.wpi.first.wpilibj.PWMVictorSPX}</li>
+ * <li>{@link edu.wpi.first.wpilibj.SD540}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Spark}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Talon}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Victor}</li>
+ * <li>{@link edu.wpi.first.wpilibj.VictorSP}</li>
+ * <li>{@link edu.wpi.first.wpilibj.SpeedControllerGroup}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Orientation</td><td>String</td><td>"HORIZONTAL"</td>
+ * <td>One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
+ * </table>
+ */
+ kSpeedController("Speed Controller"),
+ /**
+ * Displays a command with a toggle button. Pressing the button will start the command, and the
+ * button will automatically release when the command completes.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.command.Command}</li>
+ * <li>{@link edu.wpi.first.wpilibj.command.CommandGroup}</li>
+ * <li>Any custom subclass of {@code Command} or {@code CommandGroup}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kCommand("Command"),
+ /**
+ * Displays a PID command with a checkbox and an editor for the PIDF constants. Selecting the
+ * checkbox will start the command, and the checkbox will automatically deselect when the command
+ * completes.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.command.PIDCommand}</li>
+ * <li>Any custom subclass of {@code PIDCommand}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kPIDCommand("PID Command"),
+ /**
+ * Displays a PID controller with an editor for the PIDF constants and a toggle switch for
+ * enabling and disabling the controller.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.PIDController}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kPIDController("PID Controller"),
+ /**
+ * Displays an accelerometer with a number bar displaying the magnitude of the acceleration and
+ * text displaying the exact value.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.AnalogAccelerometer}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>-1</td>
+ * <td>The minimum acceleration value to display</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>1</td>
+ * <td>The maximum acceleration value to display</td></tr>
+ * <tr><td>Show text</td><td>Boolean</td><td>true</td>
+ * <td>Show or hide the acceleration values</td></tr>
+ * <tr><td>Precision</td><td>Number</td><td>2</td>
+ * <td>How many numbers to display after the decimal point</td></tr>
+ * <tr><td>Show tick marks</td><td>Boolean</td><td>false</td>
+ * <td>Show or hide the tick marks on the number bars</td></tr>
+ * </table>
+ */
+ kAccelerometer("Accelerometer"),
+ /**
+ * Displays a 3-axis accelerometer with a number bar for each axis' accleration.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.ADXL345_I2C}</li>
+ * <li>{@link edu.wpi.first.wpilibj.ADXL345_SPI}</li>
+ * <li>{@link edu.wpi.first.wpilibj.ADXL362}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Range</td><td>{@link Range}</td><td>k16G</td><td>The accelerometer range</td></tr>
+ * <tr><td>Show value</td><td>Boolean</td><td>true</td>
+ * <td>Show or hide the acceleration values</td></tr>
+ * <tr><td>Precision</td><td>Number</td><td>2</td>
+ * <td>How many numbers to display after the decimal point</td></tr>
+ * <tr><td>Show tick marks</td><td>Boolean</td><td>false</td>
+ * <td>Show or hide the tick marks on the number bars</td></tr>
+ * </table>
+ */
+ k3AxisAccelerometer("3-Axis Accelerometer"),
+ /**
+ * Displays a gyro with a dial from 0 to 360 degrees.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}</li>
+ * <li>{@link edu.wpi.first.wpilibj.AnalogGyro}</li>
+ * <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Major tick spacing</td><td>Number</td><td>45</td><td>Degrees</td></tr>
+ * <tr><td>Starting angle</td><td>Number</td><td>180</td>
+ * <td>How far to rotate the entire dial, in degrees</td></tr>
+ * <tr><td>Show tick mark ring</td><td>Boolean</td><td>true</td></tr>
+ * </table>
+ */
+ kGyro("Gyro"),
+ /**
+ * Displays a relay with toggle buttons for each supported mode (off, on, forward, reverse).
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.Relay}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kRelay("Relay"),
+ /**
+ * Displays a differential drive with a widget that displays the speed of each side of the
+ * drivebase and a vector for the direction and rotation of the drivebase. The widget will be
+ * controllable if the robot is in test mode.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.drive.DifferentialDrive}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Number of wheels</td><td>Number</td><td>4</td><td>Must be a positive even integer
+ * </td></tr>
+ * <tr><td>Wheel diameter</td><td>Number</td><td>80</td><td>Pixels</td></tr>
+ * <tr><td>Show velocity vectors</td><td>Boolean</td><td>true</td></tr>
+ * </table>
+ */
+ kDifferentialDrive("Differential Drivebase"),
+ /**
+ * Displays a mecanum drive with a widget that displays the speed of each wheel, and vectors for
+ * the direction and rotation of the drivebase. The widget will be controllable if the robot is
+ * in test mode.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.drive.MecanumDrive}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Show velocity vectors</td><td>Boolean</td><td>true</td></tr>
+ * </table>
+ */
+ kMecanumDrive("Mecanum Drivebase"),
+ /**
+ * Displays a camera stream.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.cscore.VideoSource} (as long as it is streaming on an MJPEG server)</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Show crosshair</td><td>Boolean</td><td>true</td>
+ * <td>Show or hide a crosshair on the image</td></tr>
+ * <tr><td>Crosshair color</td><td>Color</td><td>"white"</td>
+ * <td>Can be a string or a rgba integer</td></tr>
+ * <tr><td>Show controls</td><td>Boolean</td><td>true</td><td>Show or hide the stream controls
+ * </td></tr>
+ * <tr><td>Rotation</td><td>String</td><td>"NONE"</td>
+ * <td>Rotates the displayed image. One of {@code ["NONE", "QUARTER_CW", "QUARTER_CCW", "HALF"]}
+ * </td></tr>
+ * </table>
+ */
+ kCameraStream("Camera Stream"),
+ ;
+
+ private final String m_widgetName;
+
+ BuiltInWidgets(String widgetName) {
+ this.m_widgetName = widgetName;
+ }
+
+ @Override
+ public String getWidgetName() {
+ return m_widgetName;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
new file mode 100644
index 0000000..d56fde8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
+
+/**
+ * A Shuffleboard widget that handles a {@link Sendable} object such as a speed controller or
+ * sensor.
+ */
+public final class ComplexWidget extends ShuffleboardWidget<ComplexWidget> {
+ private final Sendable m_sendable;
+ private SendableBuilderImpl m_builder;
+
+ ComplexWidget(ShuffleboardContainer parent, String title, Sendable sendable) {
+ super(parent, title);
+ m_sendable = sendable;
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ if (m_builder == null) {
+ m_builder = new SendableBuilderImpl();
+ m_builder.setTable(parentTable.getSubTable(getTitle()));
+ m_sendable.initSendable(m_builder);
+ m_builder.startListeners();
+ }
+ m_builder.updateTable();
+ }
+
+ /**
+ * Enables user control of this widget in the Shuffleboard application. This method is
+ * package-private to prevent users from enabling control themselves. Has no effect if the
+ * sendable is not marked as an actuator with {@link SendableBuilder#setActuator}.
+ */
+ void enableIfActuator() {
+ if (m_builder.isActuator()) {
+ m_builder.startLiveWindowMode();
+ }
+ }
+
+ /**
+ * Disables user control of this widget in the Shuffleboard application. This method is
+ * package-private to prevent users from enabling control themselves. Has no effect if the
+ * sendable is not marked as an actuator with {@link SendableBuilder#setActuator}.
+ */
+ void disableIfActuator() {
+ if (m_builder.isActuator()) {
+ m_builder.stopLiveWindowMode();
+ }
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
new file mode 100644
index 0000000..a18e4c8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.NoSuchElementException;
+import java.util.Objects;
+import java.util.Set;
+import java.util.function.BiConsumer;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A helper class for Shuffleboard containers to handle common child operations.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+final class ContainerHelper {
+ private final ShuffleboardContainer m_container;
+ private final Set<String> m_usedTitles = new HashSet<>();
+ private final List<ShuffleboardComponent<?>> m_components = new ArrayList<>();
+ private final Map<String, ShuffleboardLayout> m_layouts = new LinkedHashMap<>();
+
+ ContainerHelper(ShuffleboardContainer container) {
+ m_container = container;
+ }
+
+ List<ShuffleboardComponent<?>> getComponents() {
+ return m_components;
+ }
+
+ ShuffleboardLayout getLayout(String title, String type) {
+ if (!m_layouts.containsKey(title)) {
+ ShuffleboardLayout layout = new ShuffleboardLayout(m_container, type, title);
+ m_components.add(layout);
+ m_layouts.put(title, layout);
+ }
+ return m_layouts.get(title);
+ }
+
+ ShuffleboardLayout getLayout(String title) {
+ ShuffleboardLayout layout = m_layouts.get(title);
+ if (layout == null) {
+ throw new NoSuchElementException("No layout has been defined with the title '" + title + "'");
+ }
+ return layout;
+ }
+
+ ComplexWidget add(String title, Sendable sendable) {
+ checkTitle(title);
+ ComplexWidget widget = new ComplexWidget(m_container, title, sendable);
+ m_components.add(widget);
+ return widget;
+ }
+
+ ComplexWidget add(Sendable sendable) throws IllegalArgumentException {
+ String name = SendableRegistry.getName(sendable);
+ if (name.isEmpty()) {
+ throw new IllegalArgumentException("Sendable must have a name");
+ }
+ return add(name, sendable);
+ }
+
+ SimpleWidget add(String title, Object defaultValue) {
+ Objects.requireNonNull(title, "Title cannot be null");
+ Objects.requireNonNull(defaultValue, "Default value cannot be null");
+ checkTitle(title);
+ checkNtType(defaultValue);
+
+ SimpleWidget widget = new SimpleWidget(m_container, title);
+ m_components.add(widget);
+ widget.getEntry().setDefaultValue(defaultValue);
+ return widget;
+ }
+
+ SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier, NetworkTableEntry::setString);
+ }
+
+ SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier::getAsDouble, NetworkTableEntry::setDouble);
+ }
+
+ SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier::getAsBoolean, NetworkTableEntry::setBoolean);
+ }
+
+ SuppliedValueWidget<String[]> addStringArray(String title, Supplier<String[]> valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier, NetworkTableEntry::setStringArray);
+ }
+
+ SuppliedValueWidget<double[]> addDoubleArray(String title, Supplier<double[]> valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier, NetworkTableEntry::setDoubleArray);
+ }
+
+ SuppliedValueWidget<boolean[]> addBooleanArray(String title, Supplier<boolean[]> valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier, NetworkTableEntry::setBooleanArray);
+ }
+
+ SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier) {
+ precheck(title, valueSupplier);
+ return addSupplied(title, valueSupplier, NetworkTableEntry::setRaw);
+ }
+
+ private void precheck(String title, Object valueSupplier) {
+ Objects.requireNonNull(title, "Title cannot be null");
+ Objects.requireNonNull(valueSupplier, "Value supplier cannot be null");
+ checkTitle(title);
+ }
+
+ private <T> SuppliedValueWidget<T> addSupplied(String title,
+ Supplier<T> supplier,
+ BiConsumer<NetworkTableEntry, T> setter) {
+ SuppliedValueWidget<T> widget = new SuppliedValueWidget<>(m_container, title, supplier, setter);
+ m_components.add(widget);
+ return widget;
+ }
+
+ private static void checkNtType(Object data) {
+ if (!NetworkTableEntry.isValidDataType(data)) {
+ throw new IllegalArgumentException(
+ "Cannot add data of type " + data.getClass().getName() + " to Shuffleboard");
+ }
+ }
+
+ private void checkTitle(String title) {
+ if (m_usedTitles.contains(title)) {
+ throw new IllegalArgumentException("Title is already in use: " + title);
+ }
+ m_usedTitles.add(title);
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
new file mode 100644
index 0000000..08f9712
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * The importance of an event marker in Shuffleboard. The exact meaning of each importance level is
+ * up for interpretation on a team-to-team basis, but users should follow the general guidelines
+ * of the various importance levels. The examples given are for reference and may be ignored or
+ * considered to be more or less important from team to team.
+ */
+public enum EventImportance {
+ // Maintainer note: this enum is mirrored in WPILibC and in Shuffleboard
+ // Modifying the enum or enum strings requires a corresponding change to the C++ enum
+ // and the enum in Shuffleboard
+
+ /**
+ * A trivial event such as a change in command state.
+ */
+ kTrivial("TRIVIAL"),
+
+ /**
+ * A low importance event such as acquisition of a game piece.
+ */
+ kLow("LOW"),
+
+ /**
+ * A "normal" importance event, such as a transition from autonomous mode to teleoperated control.
+ */
+ kNormal("NORMAL"),
+
+ /**
+ * A high-importance event such as scoring a game piece.
+ */
+ kHigh("HIGH"),
+
+ /**
+ * A critically important event such as a brownout, component failure, or software deadlock.
+ */
+ kCritical("CRITICAL");
+
+ private final String m_simpleName;
+
+ EventImportance(String simpleName) {
+ m_simpleName = simpleName;
+ }
+
+ public String getSimpleName() {
+ return m_simpleName;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
new file mode 100644
index 0000000..2d7b565
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * Represents the type of a layout in Shuffleboard. Using this is preferred over specifying raw
+ * strings, to avoid typos and having to know or look up the exact string name for a desired layout.
+ *
+ * @see BuiltInWidgets the built-in widget types
+ */
+public interface LayoutType {
+ /**
+ * Gets the string type of the layout as defined by that layout in Shuffleboard.
+ */
+ String getLayoutName();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
new file mode 100644
index 0000000..9f04274
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * Controls Shuffleboard recordings via NetworkTables.
+ */
+final class RecordingController {
+ private static final String kRecordingTableName = "/Shuffleboard/.recording/";
+ private static final String kRecordingControlKey = kRecordingTableName + "RecordData";
+ private static final String kRecordingFileNameFormatKey = kRecordingTableName + "FileNameFormat";
+ private static final String kEventMarkerTableName = kRecordingTableName + "events";
+
+ private final NetworkTableEntry m_recordingControlEntry;
+ private final NetworkTableEntry m_recordingFileNameFormatEntry;
+ private final NetworkTable m_eventsTable;
+
+ RecordingController(NetworkTableInstance ntInstance) {
+ m_recordingControlEntry = ntInstance.getEntry(kRecordingControlKey);
+ m_recordingFileNameFormatEntry = ntInstance.getEntry(kRecordingFileNameFormatKey);
+ m_eventsTable = ntInstance.getTable(kEventMarkerTableName);
+ }
+
+ public void startRecording() {
+ m_recordingControlEntry.setBoolean(true);
+ }
+
+ public void stopRecording() {
+ m_recordingControlEntry.setBoolean(false);
+ }
+
+ public void setRecordingFileNameFormat(String format) {
+ m_recordingFileNameFormatEntry.setString(format);
+ }
+
+ public void clearRecordingFileNameFormat() {
+ m_recordingFileNameFormatEntry.delete();
+ }
+
+ public void addEventMarker(String name, String description, EventImportance importance) {
+ if (name == null || name.isEmpty()) {
+ DriverStation.reportError(
+ "Shuffleboard event name was not specified", true);
+ return;
+ }
+
+ if (importance == null) {
+ DriverStation.reportError(
+ "Shuffleboard event importance was null", true);
+ return;
+ }
+
+ String eventDescription = description == null ? "" : description;
+
+ m_eventsTable.getSubTable(name)
+ .getEntry("Info")
+ .setStringArray(new String[]{eventDescription, importance.getSimpleName()});
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
new file mode 100644
index 0000000..bb7738f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.Map;
+import java.util.WeakHashMap;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A wrapper to make video sources sendable and usable from Shuffleboard.
+ */
+public final class SendableCameraWrapper implements Sendable, AutoCloseable {
+ private static final String kProtocol = "camera_server://";
+
+ private static Map<VideoSource, SendableCameraWrapper> m_wrappers = new WeakHashMap<>();
+
+ private final String m_uri;
+
+ /**
+ * Creates a new sendable wrapper. Private constructor to avoid direct instantiation with
+ * multiple wrappers floating around for the same camera.
+ *
+ * @param source the source to wrap
+ */
+ private SendableCameraWrapper(VideoSource source) {
+ String name = source.getName();
+ SendableRegistry.add(this, name);
+ m_uri = kProtocol + name;
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Gets a sendable wrapper object for the given video source, creating the wrapper if one does
+ * not already exist for the source.
+ *
+ * @param source the video source to wrap
+ * @return a sendable wrapper object for the video source, usable in Shuffleboard via
+ * {@link ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
+ */
+ public static SendableCameraWrapper wrap(VideoSource source) {
+ return m_wrappers.computeIfAbsent(source, SendableCameraWrapper::new);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.addStringProperty(".ShuffleboardURI", () -> m_uri, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
new file mode 100644
index 0000000..c88256a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
@@ -0,0 +1,198 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * The Shuffleboard class provides a mechanism with which data can be added and laid out in the
+ * Shuffleboard dashboard application from a robot program. Tabs and layouts can be specified, as
+ * well as choosing which widgets to display with and setting properties of these widgets; for
+ * example, programmers can specify a specific {@code boolean} value to be displayed with a toggle
+ * button instead of the default colored box, or set custom colors for that box.
+ *
+ * <p>For example, displaying a boolean entry with a toggle button:</p>
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ * .add("My Boolean", false)
+ * .withWidget("Toggle Button")
+ * .getEntry();
+ * }</pre>
+ *
+ * <p>Changing the colors of the boolean box:</p>
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ * .add("My Boolean", false)
+ * .withWidget("Boolean Box")
+ * .withProperties(Map.of("colorWhenTrue", "green", "colorWhenFalse", "maroon"))
+ * .getEntry();
+ * }</pre>
+ *
+ * <p>Specifying a parent layout. Note that the layout type must <i>always</i> be specified, even if
+ * the layout has already been generated by a previously defined entry.</p>
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ * .getLayout("List", "Example List")
+ * .add("My Boolean", false)
+ * .withWidget("Toggle Button")
+ * .getEntry();
+ * }</pre>
+ *
+ * <p>Teams are encouraged to set up shuffleboard layouts at the start of the robot program.</p>
+ */
+public final class Shuffleboard {
+ /**
+ * The name of the base NetworkTable into which all Shuffleboard data will be added.
+ */
+ public static final String kBaseTableName = "/Shuffleboard";
+
+ private static final ShuffleboardRoot root =
+ new ShuffleboardInstance(NetworkTableInstance.getDefault());
+ private static final RecordingController recordingController =
+ new RecordingController(NetworkTableInstance.getDefault());
+
+ private Shuffleboard() {
+ throw new UnsupportedOperationException("This is a utility class and cannot be instantiated");
+ }
+
+ /**
+ * Updates all the values in Shuffleboard. Iterative and timed robots are pre-configured to call
+ * this method in the main robot loop; teams using custom robot base classes, or subclass
+ * SampleRobot, should make sure to call this repeatedly to keep data on the dashboard up to date.
+ */
+ public static void update() {
+ root.update();
+ }
+
+ /**
+ * Gets the Shuffleboard tab with the given title, creating it if it does not already exist.
+ *
+ * @param title the title of the tab
+ * @return the tab with the given title
+ */
+ public static ShuffleboardTab getTab(String title) {
+ return root.getTab(title);
+ }
+
+ /**
+ * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i>
+ * is the number of tabs in the dashboard at the time this method is called.
+ *
+ * @param index the index of the tab to select
+ */
+ public static void selectTab(int index) {
+ root.selectTab(index);
+ }
+
+ /**
+ * Selects the tab in the dashboard with the given title.
+ *
+ * @param title the title of the tab to select
+ */
+ public static void selectTab(String title) {
+ root.selectTab(title);
+ }
+
+ /**
+ * Enables user control of widgets containing actuators: speed controllers, relays, etc. This
+ * should only be used when the robot is in test mode. IterativeRobotBase and SampleRobot are
+ * both configured to call this method when entering test mode; most users should not need to use
+ * this method directly.
+ */
+ public static void enableActuatorWidgets() {
+ root.enableActuatorWidgets();
+ }
+
+ /**
+ * Disables user control of widgets containing actuators. For safety reasons, actuators should
+ * only be controlled while in test mode. IterativeRobotBase and SampleRobot are both configured
+ * to call this method when exiting in test mode; most users should not need to use
+ * this method directly.
+ */
+ public static void disableActuatorWidgets() {
+ update(); // Need to update to make sure the sendable builders are initialized
+ root.disableActuatorWidgets();
+ }
+
+ /**
+ * Starts data recording on the dashboard. Has no effect if recording is already in progress.
+ *
+ * @see #stopRecording()
+ */
+ public static void startRecording() {
+ recordingController.startRecording();
+ }
+
+ /**
+ * Stops data recording on the dashboard. Has no effect if no recording is in progress.
+ *
+ * @see #startRecording()
+ */
+ public static void stopRecording() {
+ recordingController.stopRecording();
+ }
+
+ /**
+ * Sets the file name format for new recording files to use. If recording is in progress when this
+ * method is called, it will continue to use the same file. New recordings will use the format.
+ *
+ * <p>To avoid recording files overwriting each other, make sure to use unique recording file
+ * names. File name formats accept templates for inserting the date and time when the recording
+ * started with the {@code ${date}} and {@code ${time}} templates, respectively. For example,
+ * the default format is {@code "recording-${time}"} and recording files created with it will have
+ * names like {@code "recording-2018.01.15.sbr"}. Users are <strong>strongly</strong> recommended
+ * to use the {@code ${time}} template to ensure unique file names.
+ * </p>
+ *
+ * @param format the format for the
+ * @see #clearRecordingFileNameFormat()
+ */
+ public static void setRecordingFileNameFormat(String format) {
+ recordingController.setRecordingFileNameFormat(format);
+ }
+
+ /**
+ * Clears the custom name format for recording files. New recordings will use the default format.
+ *
+ * @see #setRecordingFileNameFormat(String)
+ */
+ public static void clearRecordingFileNameFormat() {
+ recordingController.clearRecordingFileNameFormat();
+ }
+
+ /**
+ * Notifies Shuffleboard of an event. Events can range from as trivial as a change in a command
+ * state to as critical as a total power loss or component failure. If Shuffleboard is recording,
+ * the event will also be recorded.
+ *
+ * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then
+ * no event will be sent and an error will be printed to the driver station.
+ *
+ * @param name the name of the event
+ * @param description a description of the event
+ * @param importance the importance of the event
+ */
+ public static void addEventMarker(String name, String description, EventImportance importance) {
+ recordingController.addEventMarker(name, description, importance);
+ }
+
+ /**
+ * Notifies Shuffleboard of an event. Events can range from as trivial as a change in a command
+ * state to as critical as a total power loss or component failure. If Shuffleboard is recording,
+ * the event will also be recorded.
+ *
+ * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then
+ * no event will be sent and an error will be printed to the driver station.
+ *
+ * @param name the name of the event
+ * @param importance the importance of the event
+ */
+ public static void addEventMarker(String name, EventImportance importance) {
+ addEventMarker(name, "", importance);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
new file mode 100644
index 0000000..5d90396
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.Map;
+
+import edu.wpi.first.networktables.NetworkTable;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A generic component in Shuffleboard.
+ *
+ * @param <C> the self type
+ */
+public abstract class ShuffleboardComponent<C extends ShuffleboardComponent<C>>
+ implements ShuffleboardValue {
+ private final ShuffleboardContainer m_parent;
+ private final String m_title;
+ private String m_type;
+ private Map<String, Object> m_properties;
+ private boolean m_metadataDirty = true;
+ private int m_column = -1;
+ private int m_row = -1;
+ private int m_width = -1;
+ private int m_height = -1;
+
+ protected ShuffleboardComponent(ShuffleboardContainer parent, String title, String type) {
+ m_parent = requireNonNullParam(parent, "parent", "ShuffleboardComponent");
+ m_title = requireNonNullParam(title, "title", "ShuffleboardComponent");
+ m_type = type;
+ }
+
+ protected ShuffleboardComponent(ShuffleboardContainer parent, String title) {
+ this(parent, title, null);
+ }
+
+ public final ShuffleboardContainer getParent() {
+ return m_parent;
+ }
+
+ protected final void setType(String type) {
+ m_type = type;
+ m_metadataDirty = true;
+ }
+
+ public final String getType() {
+ return m_type;
+ }
+
+ @Override
+ public final String getTitle() {
+ return m_title;
+ }
+
+ /**
+ * Gets the custom properties for this component. May be null.
+ */
+ final Map<String, Object> getProperties() {
+ return m_properties;
+ }
+
+ /**
+ * Sets custom properties for this component. Property names are case- and whitespace-insensitive
+ * (capitalization and spaces do not matter).
+ *
+ * @param properties the properties for this component
+ * @return this component
+ */
+ public final C withProperties(Map<String, Object> properties) {
+ m_properties = properties;
+ m_metadataDirty = true;
+ return (C) this;
+ }
+
+ /**
+ * Sets the position of this component in the tab. This has no effect if this component is inside
+ * a layout.
+ *
+ * <p>If the position of a single component is set, it is recommended to set the positions of
+ * <i>all</i> components inside a tab to prevent Shuffleboard from automatically placing another
+ * component there before the one with the specific position is sent.
+ *
+ * @param columnIndex the column in the tab to place this component
+ * @param rowIndex the row in the tab to place this component
+ * @return this component
+ */
+ public final C withPosition(int columnIndex, int rowIndex) {
+ m_column = columnIndex;
+ m_row = rowIndex;
+ m_metadataDirty = true;
+ return (C) this;
+ }
+
+ /**
+ * Sets the size of this component in the tab. This has no effect if this component is inside a
+ * layout.
+ *
+ * @param width how many columns wide the component should be
+ * @param height how many rows high the component should be
+ * @return this component
+ */
+ public final C withSize(int width, int height) {
+ m_width = width;
+ m_height = height;
+ m_metadataDirty = true;
+ return (C) this;
+ }
+
+ protected final void buildMetadata(NetworkTable metaTable) {
+ if (!m_metadataDirty) {
+ return;
+ }
+ // Component type
+ if (getType() == null) {
+ metaTable.getEntry("PreferredComponent").delete();
+ } else {
+ metaTable.getEntry("PreferredComponent").forceSetString(getType());
+ }
+
+ // Tile size
+ if (m_width <= 0 || m_height <= 0) {
+ metaTable.getEntry("Size").delete();
+ } else {
+ metaTable.getEntry("Size").setDoubleArray(new double[]{m_width, m_height});
+ }
+
+ // Tile position
+ if (m_column < 0 || m_row < 0) {
+ metaTable.getEntry("Position").delete();
+ } else {
+ metaTable.getEntry("Position").setDoubleArray(new double[]{m_column, m_row});
+ }
+
+ // Custom properties
+ if (getProperties() != null) {
+ NetworkTable propTable = metaTable.getSubTable("Properties");
+ getProperties().forEach((name, value) -> propTable.getEntry(name).setValue(value));
+ }
+ m_metadataDirty = false;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
new file mode 100644
index 0000000..331b7f1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
@@ -0,0 +1,248 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.List;
+import java.util.NoSuchElementException;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * Common interface for objects that can contain shuffleboard components.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public interface ShuffleboardContainer extends ShuffleboardValue {
+
+ /**
+ * Gets the components that are direct children of this container.
+ */
+ List<ShuffleboardComponent<?>> getComponents();
+
+ /**
+ * Gets the layout with the given type and title, creating it if it does not already exist at the
+ * time this method is called. Note: this method should only be used to use a layout type that
+ * is not already built into Shuffleboard. To use a layout built into Shuffleboard, use
+ * {@link #getLayout(String, LayoutType)} and the layouts in {@link BuiltInLayouts}.
+ *
+ * @param title the title of the layout
+ * @param type the type of the layout, eg "List Layout" or "Grid Layout"
+ * @return the layout
+ * @see #getLayout(String, LayoutType)
+ */
+ ShuffleboardLayout getLayout(String title, String type);
+
+ /**
+ * Gets the layout with the given type and title, creating it if it does not already exist at the
+ * time this method is called.
+ *
+ * @param title the title of the layout
+ * @param layoutType the type of the layout, eg "List" or "Grid"
+ * @return the layout
+ */
+ default ShuffleboardLayout getLayout(String title, LayoutType layoutType) {
+ return getLayout(title, layoutType.getLayoutName());
+ }
+
+ /**
+ * Gets the already-defined layout in this container with the given title.
+ *
+ * <pre>{@code
+ * Shuffleboard.getTab("Example Tab")
+ * .getLayout("My Layout", BuiltInLayouts.kList);
+ *
+ * // Later...
+ * Shuffleboard.getTab("Example Tab")
+ * .getLayout("My Layout");
+ * }</pre>
+ *
+ * @param title the title of the layout to get
+ * @return the layout with the given title
+ * @throws NoSuchElementException if no layout has yet been defined with the given title
+ */
+ ShuffleboardLayout getLayout(String title) throws NoSuchElementException;
+
+ /**
+ * Adds a widget to this container to display the given sendable.
+ *
+ * @param title the title of the widget
+ * @param sendable the sendable to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ ComplexWidget add(String title, Sendable sendable) throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container to display the given video stream.
+ *
+ * @param title the title of the widget
+ * @param video the video stream to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ default ComplexWidget add(String title, VideoSource video) throws IllegalArgumentException {
+ return add(title, SendableCameraWrapper.wrap(video));
+ }
+
+ /**
+ * Adds a widget to this container to display the given sendable.
+ *
+ * @param sendable the sendable to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title, or if the sendable's name has not been specified
+ */
+ ComplexWidget add(Sendable sendable);
+
+ /**
+ * Adds a widget to this container to display the given video stream.
+ *
+ * @param video the video to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the same
+ * title as the video source
+ */
+ default ComplexWidget add(VideoSource video) {
+ return add(SendableCameraWrapper.wrap(video));
+ }
+
+ /**
+ * Adds a widget to this container to display the given data.
+ *
+ * @param title the title of the widget
+ * @param defaultValue the default value of the widget
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+ */
+ SimpleWidget add(String title, Object defaultValue) throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<String[]> addStringArray(String title, Supplier<String[]> valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<double[]> addDoubleArray(String title, Supplier<double[]> valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<boolean[]> addBooleanArray(String title, Supplier<boolean[]> valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided by the value
+ * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+ * overridden by values from the value supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier)
+ throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container to display a simple piece of data. Unlike
+ * {@link #add(String, Object)}, the value in the widget will be saved on the robot and will be
+ * used when the robot program next starts rather than {@code defaultValue}.
+ *
+ * @param title the title of the widget
+ * @param defaultValue the default value of the widget
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ * @see #add(String, Object) add(String title, Object defaultValue)
+ */
+ default SimpleWidget addPersistent(String title, Object defaultValue)
+ throws IllegalArgumentException {
+ SimpleWidget widget = add(title, defaultValue);
+ widget.getEntry().setPersistent();
+ return widget;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
new file mode 100644
index 0000000..8af1d78
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.LinkedHashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+final class ShuffleboardInstance implements ShuffleboardRoot {
+ private final Map<String, ShuffleboardTab> m_tabs = new LinkedHashMap<>();
+
+ private boolean m_tabsChanged = false; // NOPMD redundant field initializer
+ private final NetworkTable m_rootTable;
+ private final NetworkTable m_rootMetaTable;
+ private final NetworkTableEntry m_selectedTabEntry;
+
+ /**
+ * Creates a new Shuffleboard instance.
+ *
+ * @param ntInstance the NetworkTables instance to use
+ */
+ ShuffleboardInstance(NetworkTableInstance ntInstance) {
+ requireNonNullParam(ntInstance, "ntInstance", "ShuffleboardInstance");
+ m_rootTable = ntInstance.getTable(Shuffleboard.kBaseTableName);
+ m_rootMetaTable = m_rootTable.getSubTable(".metadata");
+ m_selectedTabEntry = m_rootMetaTable.getEntry("Selected");
+ HAL.report(tResourceType.kResourceType_Shuffleboard, 0);
+ }
+
+ @Override
+ public ShuffleboardTab getTab(String title) {
+ requireNonNullParam(title, "title", "getTab");
+ if (!m_tabs.containsKey(title)) {
+ m_tabs.put(title, new ShuffleboardTab(this, title));
+ m_tabsChanged = true;
+ }
+ return m_tabs.get(title);
+ }
+
+ @Override
+ public void update() {
+ if (m_tabsChanged) {
+ String[] tabTitles = m_tabs.values()
+ .stream()
+ .map(ShuffleboardTab::getTitle)
+ .toArray(String[]::new);
+ m_rootMetaTable.getEntry("Tabs").forceSetStringArray(tabTitles);
+ m_tabsChanged = false;
+ }
+ for (ShuffleboardTab tab : m_tabs.values()) {
+ String title = tab.getTitle();
+ tab.buildInto(m_rootTable, m_rootMetaTable.getSubTable(title));
+ }
+ }
+
+ @Override
+ public void enableActuatorWidgets() {
+ applyToAllComplexWidgets(ComplexWidget::enableIfActuator);
+ }
+
+ @Override
+ public void disableActuatorWidgets() {
+ applyToAllComplexWidgets(ComplexWidget::disableIfActuator);
+ }
+
+ @Override
+ public void selectTab(int index) {
+ m_selectedTabEntry.forceSetDouble(index);
+ }
+
+ @Override
+ public void selectTab(String title) {
+ m_selectedTabEntry.forceSetString(title);
+ }
+
+ /**
+ * Applies the function {@code func} to all complex widgets in this root, regardless of how they
+ * are nested.
+ *
+ * @param func the function to apply to all complex widgets
+ */
+ private void applyToAllComplexWidgets(Consumer<ComplexWidget> func) {
+ for (ShuffleboardTab tab : m_tabs.values()) {
+ apply(tab, func);
+ }
+ }
+
+ /**
+ * Applies the function {@code func} to all complex widgets in {@code container}. Helper method
+ * for {@link #applyToAllComplexWidgets}.
+ */
+ private void apply(ShuffleboardContainer container, Consumer<ComplexWidget> func) {
+ for (ShuffleboardComponent<?> component : container.getComponents()) {
+ if (component instanceof ComplexWidget) {
+ func.accept((ComplexWidget) component);
+ }
+ if (component instanceof ShuffleboardContainer) {
+ apply((ShuffleboardContainer) component, func);
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
new file mode 100644
index 0000000..8f3014a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.List;
+import java.util.NoSuchElementException;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A layout in a Shuffleboard tab. Layouts can contain widgets and other layouts.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class ShuffleboardLayout extends ShuffleboardComponent<ShuffleboardLayout>
+ implements ShuffleboardContainer {
+ private final ContainerHelper m_helper = new ContainerHelper(this);
+
+ ShuffleboardLayout(ShuffleboardContainer parent, String name, String type) {
+ super(parent, requireNonNullParam(type, "type", "ShuffleboardLayout"), name);
+ }
+
+ @Override
+ public List<ShuffleboardComponent<?>> getComponents() {
+ return m_helper.getComponents();
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title, String type) {
+ return m_helper.getLayout(title, type);
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title) throws NoSuchElementException {
+ return m_helper.getLayout(title);
+ }
+
+ @Override
+ public ComplexWidget add(String title, Sendable sendable) throws IllegalArgumentException {
+ return m_helper.add(title, sendable);
+ }
+
+ @Override
+ public ComplexWidget add(Sendable sendable) throws IllegalArgumentException {
+ return m_helper.add(sendable);
+ }
+
+ @Override
+ public SimpleWidget add(String title, Object defaultValue) throws IllegalArgumentException {
+ return m_helper.add(title, defaultValue);
+ }
+
+ @Override
+ public SuppliedValueWidget<String> addString(String title,
+ Supplier<String> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addString(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<Double> addNumber(String title,
+ DoubleSupplier valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addNumber(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<Boolean> addBoolean(String title,
+ BooleanSupplier valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addBoolean(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<String[]> addStringArray(String title,
+ Supplier<String[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addStringArray(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<double[]> addDoubleArray(String title,
+ Supplier<double[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addDoubleArray(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<boolean[]> addBooleanArray(String title,
+ Supplier<boolean[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addBooleanArray(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<byte[]> addRaw(String title,
+ Supplier<byte[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addRaw(title, valueSupplier);
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ NetworkTable table = parentTable.getSubTable(getTitle());
+ table.getEntry(".type").setString("ShuffleboardLayout");
+ for (ShuffleboardComponent<?> component : getComponents()) {
+ component.buildInto(table, metaTable.getSubTable(component.getTitle()));
+ }
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
new file mode 100644
index 0000000..a32af69
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * The root of the data placed in Shuffleboard. It contains the tabs, but no data is placed
+ * directly in the root.
+ *
+ * <p>This class is package-private to minimize API surface area.
+ */
+interface ShuffleboardRoot {
+
+ /**
+ * Gets the tab with the given title, creating it if it does not already exist.
+ *
+ * @param title the title of the tab
+ * @return the tab with the given title
+ */
+ ShuffleboardTab getTab(String title);
+
+ /**
+ * Updates all tabs.
+ */
+ void update();
+
+ /**
+ * Enables all widgets in Shuffleboard that offer user control over actuators.
+ */
+ void enableActuatorWidgets();
+
+ /**
+ * Disables all widgets in Shuffleboard that offer user control over actuators.
+ */
+ void disableActuatorWidgets();
+
+ /**
+ * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i>
+ * is the number of tabs in the dashboard at the time this method is called.
+ *
+ * @param index the index of the tab to select
+ */
+ void selectTab(int index);
+
+ /**
+ * Selects the tab in the dashboard with the given title.
+ *
+ * @param title the title of the tab to select
+ */
+ void selectTab(String title);
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
new file mode 100644
index 0000000..b46c6d2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.List;
+import java.util.NoSuchElementException;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * Represents a tab in the Shuffleboard dashboard. Widgets can be added to the tab with
+ * {@link #add(Sendable)}, {@link #add(String, Object)}, and {@link #add(String, Sendable)}. Widgets
+ * can also be added to layouts with {@link #getLayout(String, String)}; layouts can be nested
+ * arbitrarily deep (note that too many levels may make deeper components unusable).
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public final class ShuffleboardTab implements ShuffleboardContainer {
+ private final ContainerHelper m_helper = new ContainerHelper(this);
+ private final ShuffleboardRoot m_root;
+ private final String m_title;
+
+ ShuffleboardTab(ShuffleboardRoot root, String title) {
+ m_root = root;
+ m_title = title;
+ }
+
+ @Override
+ public String getTitle() {
+ return m_title;
+ }
+
+ ShuffleboardRoot getRoot() {
+ return m_root;
+ }
+
+ @Override
+ public List<ShuffleboardComponent<?>> getComponents() {
+ return m_helper.getComponents();
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title, String type) {
+ return m_helper.getLayout(title, type);
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title) throws NoSuchElementException {
+ return m_helper.getLayout(title);
+ }
+
+ @Override
+ public ComplexWidget add(String title, Sendable sendable) {
+ return m_helper.add(title, sendable);
+ }
+
+ @Override
+ public ComplexWidget add(Sendable sendable) throws IllegalArgumentException {
+ return m_helper.add(sendable);
+ }
+
+ @Override
+ public SimpleWidget add(String title, Object defaultValue) {
+ return m_helper.add(title, defaultValue);
+ }
+
+ @Override
+ public SuppliedValueWidget<String> addString(String title,
+ Supplier<String> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addString(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<Double> addNumber(String title,
+ DoubleSupplier valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addNumber(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<Boolean> addBoolean(String title,
+ BooleanSupplier valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addBoolean(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<String[]> addStringArray(String title,
+ Supplier<String[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addStringArray(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<double[]> addDoubleArray(String title,
+ Supplier<double[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addDoubleArray(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<boolean[]> addBooleanArray(String title,
+ Supplier<boolean[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addBooleanArray(title, valueSupplier);
+ }
+
+ @Override
+ public SuppliedValueWidget<byte[]> addRaw(String title,
+ Supplier<byte[]> valueSupplier)
+ throws IllegalArgumentException {
+ return m_helper.addRaw(title, valueSupplier);
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ NetworkTable tabTable = parentTable.getSubTable(m_title);
+ tabTable.getEntry(".type").setString("ShuffleboardTab");
+ for (ShuffleboardComponent<?> component : m_helper.getComponents()) {
+ component.buildInto(tabTable, metaTable.getSubTable(component.getTitle()));
+ }
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
new file mode 100644
index 0000000..8bfa6c3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+
+interface ShuffleboardValue {
+
+ /**
+ * Gets the title of this Shuffleboard value.
+ */
+ String getTitle();
+
+ /**
+ * Builds the entries for this value.
+ *
+ * @param parentTable the table containing all the data for the parent. Values that require a
+ * complex entry or table structure should call {@code
+ * parentTable.getSubTable(getTitle())} to get the table to put data into.
+ * Values that only use a single entry should call {@code
+ * parentTable.getEntry(getTitle())} to get that entry.
+ * @param metaTable the table containing all the metadata for this value and its sub-values
+ */
+ void buildInto(NetworkTable parentTable, NetworkTable metaTable);
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
new file mode 100644
index 0000000..3dc6622
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * Abstract superclass for widgets.
+ *
+ * <p>This class is package-private to minimize API surface area.
+ *
+ * @param <W> the self type
+ */
+abstract class ShuffleboardWidget<W extends ShuffleboardWidget<W>>
+ extends ShuffleboardComponent<W> {
+
+ ShuffleboardWidget(ShuffleboardContainer parent, String title) {
+ super(parent, title);
+ }
+
+ /**
+ * Sets the type of widget used to display the data. If not set, the default widget type will be
+ * used.
+ *
+ * @param widgetType the type of the widget used to display the data
+ * @return this widget object
+ * @see BuiltInWidgets
+ */
+ public final W withWidget(WidgetType widgetType) {
+ return withWidget(widgetType.getWidgetName());
+ }
+
+ /**
+ * Sets the type of widget used to display the data. If not set, the default widget type will be
+ * used. This method should only be used to use a widget that does not come built into
+ * Shuffleboard (i.e. one that comes with a custom or thrid-party plugin). To use a widget that
+ * is built into Shuffleboard, use {@link #withWidget(WidgetType)} and {@link BuiltInWidgets}.
+ *
+ * @param widgetType the type of the widget used to display the data
+ * @return this widget object
+ */
+ public final W withWidget(String widgetType) {
+ setType(widgetType);
+ return (W) this;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
new file mode 100644
index 0000000..c645baa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+/**
+ * A Shuffleboard widget that handles a single data point such as a number or string.
+ */
+public final class SimpleWidget extends ShuffleboardWidget<SimpleWidget> {
+ private NetworkTableEntry m_entry;
+
+ SimpleWidget(ShuffleboardContainer parent, String title) {
+ super(parent, title);
+ }
+
+ /**
+ * Gets the NetworkTable entry that contains the data for this widget.
+ */
+ public NetworkTableEntry getEntry() {
+ if (m_entry == null) {
+ forceGenerate();
+ }
+ return m_entry;
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ if (m_entry == null) {
+ m_entry = parentTable.getEntry(getTitle());
+ }
+ }
+
+ private void forceGenerate() {
+ ShuffleboardContainer parent = getParent();
+ while (parent instanceof ShuffleboardLayout) {
+ parent = ((ShuffleboardLayout) parent).getParent();
+ }
+ ShuffleboardTab tab = (ShuffleboardTab) parent;
+ tab.getRoot().update();
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
new file mode 100644
index 0000000..c7c9e8b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.function.BiConsumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+/**
+ * A Shuffleboard widget whose value is provided by user code.
+ *
+ * @param <T> the type of values in the widget
+ */
+public final class SuppliedValueWidget<T> extends ShuffleboardWidget<SuppliedValueWidget<T>> {
+ private final Supplier<T> m_supplier;
+ private final BiConsumer<NetworkTableEntry, T> m_setter;
+
+ /**
+ * Package-private constructor for use by the Shuffleboard API.
+ *
+ * @param parent the parent container for the widget
+ * @param title the title of the widget
+ * @param supplier the supplier for values to place in the NetworkTable entry
+ * @param setter the function for placing values in the NetworkTable entry
+ */
+ SuppliedValueWidget(ShuffleboardContainer parent,
+ String title,
+ Supplier<T> supplier,
+ BiConsumer<NetworkTableEntry, T> setter) {
+ super(parent, title);
+ this.m_supplier = supplier;
+ this.m_setter = setter;
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ metaTable.getEntry("Controllable").setBoolean(false);
+
+ NetworkTableEntry entry = parentTable.getEntry(getTitle());
+ m_setter.accept(entry, m_supplier.get());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
new file mode 100644
index 0000000..1e8242f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * Represents the type of a widget in Shuffleboard. Using this is preferred over specifying raw
+ * strings, to avoid typos and having to know or look up the exact string name for a desired widget.
+ *
+ * @see BuiltInWidgets the built-in widget types
+ */
+public interface WidgetType {
+ /**
+ * Gets the string type of the widget as defined by that widget in Shuffleboard.
+ */
+ String getWidgetName();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
new file mode 100644
index 0000000..274c7a8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.concurrent.Executor;
+
+/**
+ * An executor for running listener tasks posted by {@link edu.wpi.first.wpilibj.Sendable} listeners
+ * synchronously from the main application thread.
+ */
+class ListenerExecutor implements Executor {
+ private final Collection<Runnable> m_tasks = new ArrayList<>();
+ private final Object m_lock = new Object();
+
+ /**
+ * Posts a task to the executor to be run synchronously from the main thread.
+ *
+ * @param task The task to run synchronously from the main thread.
+ */
+ @Override
+ public void execute(Runnable task) {
+ synchronized (m_lock) {
+ m_tasks.add(task);
+ }
+ }
+
+ /**
+ * Runs all posted tasks. Called periodically from main thread.
+ */
+ public void runListenerTasks() {
+ // Locally copy tasks from internal list; minimizes blocking time
+ Collection<Runnable> tasks = new ArrayList<>();
+ synchronized (m_lock) {
+ tasks.addAll(m_tasks);
+ m_tasks.clear();
+ }
+
+ // Run all tasks
+ for (Runnable task : tasks) {
+ task.run();
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java
new file mode 100644
index 0000000..35a8b54
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableValue;
+
+public interface SendableBuilder {
+ /**
+ * Set the string representation of the named data type that will be used
+ * by the smart dashboard for this sendable.
+ *
+ * @param type data type
+ */
+ void setSmartDashboardType(String type);
+
+ /**
+ * Set a flag indicating if this sendable should be treated as an actuator.
+ * By default this flag is false.
+ *
+ * @param value true if actuator, false if not
+ */
+ void setActuator(boolean value);
+
+ /**
+ * Set the function that should be called to set the Sendable into a safe
+ * state. This is called when entering and exiting Live Window mode.
+ *
+ * @param func function
+ */
+ void setSafeState(Runnable func);
+
+ /**
+ * Set the function that should be called to update the network table
+ * for things other than properties. Note this function is not passed
+ * the network table object; instead it should use the entry handles
+ * returned by getEntry().
+ *
+ * @param func function
+ */
+ void setUpdateTable(Runnable func);
+
+ /**
+ * Add a property without getters or setters. This can be used to get
+ * entry handles for the function called by setUpdateTable().
+ *
+ * @param key property name
+ * @return Network table entry
+ */
+ NetworkTableEntry getEntry(String key);
+
+ /**
+ * Represents an operation that accepts a single boolean-valued argument and
+ * returns no result. This is the primitive type specialization of Consumer
+ * for boolean. Unlike most other functional interfaces, BooleanConsumer is
+ * expected to operate via side-effects.
+ *
+ * <p>This is a functional interface whose functional method is accept(boolean).
+ */
+ @FunctionalInterface
+ interface BooleanConsumer {
+ /**
+ * Performs the operation on the given value.
+ * @param value the value
+ */
+ void accept(boolean value);
+ }
+
+ /**
+ * Add a boolean property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter);
+
+ /**
+ * Add a double property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter);
+
+ /**
+ * Add a string property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter);
+
+ /**
+ * Add a boolean array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addBooleanArrayProperty(String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter);
+
+ /**
+ * Add a double array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addDoubleArrayProperty(String key, Supplier<double[]> getter, Consumer<double[]> setter);
+
+ /**
+ * Add a string array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addStringArrayProperty(String key, Supplier<String[]> getter, Consumer<String[]> setter);
+
+ /**
+ * Add a raw property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter);
+
+ /**
+ * Add a NetworkTableValue property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addValueProperty(String key, Supplier<NetworkTableValue> getter,
+ Consumer<NetworkTableValue> setter);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
new file mode 100644
index 0000000..e0bb4f9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
@@ -0,0 +1,410 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Function;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableValue;
+
+@SuppressWarnings("PMD.TooManyMethods")
+public class SendableBuilderImpl implements SendableBuilder {
+ private static class Property {
+ Property(NetworkTable table, String key) {
+ m_entry = table.getEntry(key);
+ }
+
+ @Override
+ @SuppressWarnings("NoFinalizer")
+ protected synchronized void finalize() {
+ stopListener();
+ }
+
+ void startListener() {
+ if (m_entry.isValid() && m_listener == 0 && m_createListener != null) {
+ m_listener = m_createListener.apply(m_entry);
+ }
+ }
+
+ void stopListener() {
+ if (m_entry.isValid() && m_listener != 0) {
+ m_entry.removeListener(m_listener);
+ m_listener = 0;
+ }
+ }
+
+ final NetworkTableEntry m_entry;
+ int m_listener;
+ Consumer<NetworkTableEntry> m_update;
+ Function<NetworkTableEntry, Integer> m_createListener;
+ }
+
+ private final List<Property> m_properties = new ArrayList<>();
+ private Runnable m_safeState;
+ private Runnable m_updateTable;
+ private NetworkTable m_table;
+ private NetworkTableEntry m_controllableEntry;
+ private boolean m_actuator;
+
+ /**
+ * Set the network table. Must be called prior to any Add* functions being called.
+ *
+ * @param table Network table
+ */
+ public void setTable(NetworkTable table) {
+ m_table = table;
+ m_controllableEntry = table.getEntry(".controllable");
+ }
+
+ /**
+ * Get the network table.
+ *
+ * @return The network table
+ */
+ public NetworkTable getTable() {
+ return m_table;
+ }
+
+ /**
+ * Return whether this sendable has an associated table.
+ * @return True if it has a table, false if not.
+ */
+ public boolean hasTable() {
+ return m_table != null;
+ }
+
+ /**
+ * Return whether this sendable should be treated as an actuator.
+ *
+ * @return True if actuator, false if not.
+ */
+ public boolean isActuator() {
+ return m_actuator;
+ }
+
+ /**
+ * Update the network table values by calling the getters for all properties.
+ */
+ public void updateTable() {
+ for (Property property : m_properties) {
+ if (property.m_update != null) {
+ property.m_update.accept(property.m_entry);
+ }
+ }
+ if (m_updateTable != null) {
+ m_updateTable.run();
+ }
+ }
+
+ /**
+ * Hook setters for all properties.
+ */
+ public void startListeners() {
+ for (Property property : m_properties) {
+ property.startListener();
+ }
+ if (m_controllableEntry != null) {
+ m_controllableEntry.setBoolean(true);
+ }
+ }
+
+ /**
+ * Unhook setters for all properties.
+ */
+ public void stopListeners() {
+ for (Property property : m_properties) {
+ property.stopListener();
+ }
+ if (m_controllableEntry != null) {
+ m_controllableEntry.setBoolean(false);
+ }
+ }
+
+ /**
+ * Start LiveWindow mode by hooking the setters for all properties. Also calls the safeState
+ * function if one was provided.
+ */
+ public void startLiveWindowMode() {
+ if (m_safeState != null) {
+ m_safeState.run();
+ }
+ startListeners();
+ }
+
+ /**
+ * Stop LiveWindow mode by unhooking the setters for all properties. Also calls the safeState
+ * function if one was provided.
+ */
+ public void stopLiveWindowMode() {
+ stopListeners();
+ if (m_safeState != null) {
+ m_safeState.run();
+ }
+ }
+
+ /**
+ * Clear properties.
+ */
+ public void clearProperties() {
+ stopListeners();
+ m_properties.clear();
+ }
+
+ /**
+ * Set the string representation of the named data type that will be used by the smart dashboard
+ * for this sendable.
+ *
+ * @param type data type
+ */
+ @Override
+ public void setSmartDashboardType(String type) {
+ m_table.getEntry(".type").setString(type);
+ }
+
+ /**
+ * Set a flag indicating if this sendable should be treated as an actuator. By default this flag
+ * is false.
+ *
+ * @param value true if actuator, false if not
+ */
+ @Override
+ public void setActuator(boolean value) {
+ m_table.getEntry(".actuator").setBoolean(value);
+ m_actuator = value;
+ }
+
+ /**
+ * Set the function that should be called to set the Sendable into a safe state. This is called
+ * when entering and exiting Live Window mode.
+ *
+ * @param func function
+ */
+ @Override
+ public void setSafeState(Runnable func) {
+ m_safeState = func;
+ }
+
+ /**
+ * Set the function that should be called to update the network table for things other than
+ * properties. Note this function is not passed the network table object; instead it should use
+ * the entry handles returned by getEntry().
+ *
+ * @param func function
+ */
+ @Override
+ public void setUpdateTable(Runnable func) {
+ m_updateTable = func;
+ }
+
+ /**
+ * Add a property without getters or setters. This can be used to get entry handles for the
+ * function called by setUpdateTable().
+ *
+ * @param key property name
+ * @return Network table entry
+ */
+ @Override
+ public NetworkTableEntry getEntry(String key) {
+ return m_table.getEntry(key);
+ }
+
+ /**
+ * Add a boolean property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setBoolean(getter.getAsBoolean());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isBoolean()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getBoolean()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a double property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setDouble(getter.getAsDouble());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isDouble()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getDouble()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a string property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setString(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isString()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getString()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a boolean array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addBooleanArrayProperty(String key, Supplier<boolean[]> getter,
+ Consumer<boolean[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setBooleanArray(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isBooleanArray()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getBooleanArray()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a double array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addDoubleArrayProperty(String key, Supplier<double[]> getter,
+ Consumer<double[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setDoubleArray(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isDoubleArray()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getDoubleArray()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a string array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addStringArrayProperty(String key, Supplier<String[]> getter,
+ Consumer<String[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setStringArray(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isStringArray()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getStringArray()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a raw property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setRaw(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isRaw()) {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value.getRaw()));
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a NetworkTableValue property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addValueProperty(String key, Supplier<NetworkTableValue> getter,
+ Consumer<NetworkTableValue> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setValue(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ SmartDashboard.postListenerTask(() -> setter.accept(event.value));
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
new file mode 100644
index 0000000..cdf1ffb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.Sendable;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * The {@link SendableChooser} class is a useful tool for presenting a selection of options to the
+ * {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
+ * this by putting every possible Command you want to run as an autonomous into a {@link
+ * SendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
+ * on the laptop. Once autonomous starts, simply ask the {@link SendableChooser} what the selected
+ * value is.
+ *
+ * @param <V> The type of the values to be stored
+ */
+public class SendableChooser<V> implements Sendable, AutoCloseable {
+ /**
+ * The key for the default value.
+ */
+ private static final String DEFAULT = "default";
+ /**
+ * The key for the selected option.
+ */
+ private static final String SELECTED = "selected";
+ /**
+ * The key for the active option.
+ */
+ private static final String ACTIVE = "active";
+ /**
+ * The key for the option array.
+ */
+ private static final String OPTIONS = "options";
+ /**
+ * The key for the instance number.
+ */
+ private static final String INSTANCE = ".instance";
+ /**
+ * A map linking strings to the objects the represent.
+ */
+ private final Map<String, V> m_map = new LinkedHashMap<>();
+ private String m_defaultChoice = "";
+ private final int m_instance;
+ private static final AtomicInteger s_instances = new AtomicInteger();
+
+ /**
+ * Instantiates a {@link SendableChooser}.
+ */
+ public SendableChooser() {
+ m_instance = s_instances.getAndIncrement();
+ SendableRegistry.add(this, "SendableChooser", m_instance);
+ }
+
+ @Override
+ public void close() {
+ SendableRegistry.remove(this);
+ }
+
+ /**
+ * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
+ * object will appear as the given name.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ public void addOption(String name, V object) {
+ m_map.put(name, object);
+ }
+
+ /**
+ * Adds the given object to the list of options.
+ *
+ * @deprecated Use {@link #addOption(String, Object)} instead.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ @Deprecated
+ public void addObject(String name, V object) {
+ addOption(name, object);
+ }
+
+ /**
+ * Adds the given object to the list of options and marks it as the default. Functionally, this is
+ * very close to {@link #addOption(String, Object)} except that it will use this as the default
+ * option if none other is explicitly selected.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ public void setDefaultOption(String name, V object) {
+ requireNonNullParam(name, "name", "setDefaultOption");
+
+ m_defaultChoice = name;
+ addOption(name, object);
+ }
+
+ /**
+ * Adds the given object to the list of options and marks it as the default.
+ *
+ * @deprecated Use {@link #setDefaultOption(String, Object)} instead.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ @Deprecated
+ public void addDefault(String name, V object) {
+ setDefaultOption(name, object);
+ }
+
+ /**
+ * Returns the selected option. If there is none selected, it will return the default. If there is
+ * none selected and no default, then it will return {@code null}.
+ *
+ * @return the option selected
+ */
+ public V getSelected() {
+ m_mutex.lock();
+ try {
+ if (m_selected != null) {
+ return m_map.get(m_selected);
+ } else {
+ return m_map.get(m_defaultChoice);
+ }
+ } finally {
+ m_mutex.unlock();
+ }
+ }
+
+ private String m_selected;
+ private final List<NetworkTableEntry> m_activeEntries = new ArrayList<>();
+ private final ReentrantLock m_mutex = new ReentrantLock();
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("String Chooser");
+ builder.getEntry(INSTANCE).setDouble(m_instance);
+ builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
+ builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
+ builder.addStringProperty(ACTIVE, () -> {
+ m_mutex.lock();
+ try {
+ if (m_selected != null) {
+ return m_selected;
+ } else {
+ return m_defaultChoice;
+ }
+ } finally {
+ m_mutex.unlock();
+ }
+ }, null);
+ m_mutex.lock();
+ try {
+ m_activeEntries.add(builder.getEntry(ACTIVE));
+ } finally {
+ m_mutex.unlock();
+ }
+ builder.addStringProperty(SELECTED, null, val -> {
+ m_mutex.lock();
+ try {
+ m_selected = val;
+ for (NetworkTableEntry entry : m_activeEntries) {
+ entry.setString(val);
+ }
+ } finally {
+ m_mutex.unlock();
+ }
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
new file mode 100644
index 0000000..a8cd557
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
@@ -0,0 +1,521 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.lang.ref.WeakReference;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.Map;
+import java.util.WeakHashMap;
+import java.util.function.Consumer;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Sendable;
+
+
+/**
+ * The SendableRegistry class is the public interface for registering sensors
+ * and actuators for use on dashboards and LiveWindow.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class SendableRegistry {
+ private static class Component {
+ Component() {}
+
+ Component(Sendable sendable) {
+ m_sendable = new WeakReference<>(sendable);
+ }
+
+ WeakReference<Sendable> m_sendable;
+ SendableBuilderImpl m_builder = new SendableBuilderImpl();
+ String m_name;
+ String m_subsystem = "Ungrouped";
+ WeakReference<Sendable> m_parent;
+ boolean m_liveWindow;
+ Object[] m_data;
+
+ void setName(String moduleType, int channel) {
+ m_name = moduleType + "[" + channel + "]";
+ }
+
+ void setName(String moduleType, int moduleNumber, int channel) {
+ m_name = moduleType + "[" + moduleNumber + "," + channel + "]";
+ }
+ }
+
+ private static final Map<Object, Component> components = new WeakHashMap<>();
+ private static int nextDataHandle;
+
+ private static Component getOrAdd(Sendable sendable) {
+ Component comp = components.get(sendable);
+ if (comp == null) {
+ comp = new Component(sendable);
+ components.put(sendable, comp);
+ } else {
+ if (comp.m_sendable == null) {
+ comp.m_sendable = new WeakReference<>(sendable);
+ }
+ }
+ return comp;
+ }
+
+ private SendableRegistry() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param name component name
+ */
+ public static synchronized void add(Sendable sendable, String name) {
+ Component comp = getOrAdd(sendable);
+ comp.m_name = name;
+ }
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ public static synchronized void add(Sendable sendable, String moduleType, int channel) {
+ Component comp = getOrAdd(sendable);
+ comp.setName(moduleType, channel);
+ }
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into
+ */
+ public static synchronized void add(Sendable sendable, String moduleType, int moduleNumber,
+ int channel) {
+ Component comp = getOrAdd(sendable);
+ comp.setName(moduleType, moduleNumber, channel);
+ }
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param subsystem subsystem name
+ * @param name component name
+ */
+ public static synchronized void add(Sendable sendable, String subsystem, String name) {
+ Component comp = getOrAdd(sendable);
+ comp.m_name = name;
+ comp.m_subsystem = subsystem;
+ }
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param name component name
+ */
+ public static synchronized void addLW(Sendable sendable, String name) {
+ Component comp = getOrAdd(sendable);
+ comp.m_liveWindow = true;
+ comp.m_name = name;
+ }
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ public static synchronized void addLW(Sendable sendable, String moduleType, int channel) {
+ Component comp = getOrAdd(sendable);
+ comp.m_liveWindow = true;
+ comp.setName(moduleType, channel);
+ }
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into
+ */
+ public static synchronized void addLW(Sendable sendable, String moduleType, int moduleNumber,
+ int channel) {
+ Component comp = getOrAdd(sendable);
+ comp.m_liveWindow = true;
+ comp.setName(moduleType, moduleNumber, channel);
+ }
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param subsystem subsystem name
+ * @param name component name
+ */
+ public static synchronized void addLW(Sendable sendable, String subsystem, String name) {
+ Component comp = getOrAdd(sendable);
+ comp.m_liveWindow = true;
+ comp.m_name = name;
+ comp.m_subsystem = subsystem;
+ }
+
+ /**
+ * Adds a child object to an object. Adds the child object to the registry
+ * if it's not already present.
+ *
+ * @param parent parent object
+ * @param child child object
+ */
+ public static synchronized void addChild(Sendable parent, Object child) {
+ Component comp = components.get(child);
+ if (comp == null) {
+ comp = new Component();
+ components.put(child, comp);
+ }
+ comp.m_parent = new WeakReference<>(parent);
+ }
+
+ /**
+ * Removes an object from the registry.
+ *
+ * @param sendable object to remove
+ * @return true if the object was removed; false if it was not present
+ */
+ public static synchronized boolean remove(Sendable sendable) {
+ return components.remove(sendable) != null;
+ }
+
+ /**
+ * Determines if an object is in the registry.
+ *
+ * @param sendable object to check
+ * @return True if in registry, false if not.
+ */
+ public static synchronized boolean contains(Sendable sendable) {
+ return components.containsKey(sendable);
+ }
+
+ /**
+ * Gets the name of an object.
+ *
+ * @param sendable object
+ * @return Name (empty if object is not in registry)
+ */
+ public static synchronized String getName(Sendable sendable) {
+ Component comp = components.get(sendable);
+ if (comp == null) {
+ return "";
+ }
+ return comp.m_name;
+ }
+
+ /**
+ * Sets the name of an object.
+ *
+ * @param sendable object
+ * @param name name
+ */
+ public static synchronized void setName(Sendable sendable, String name) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.m_name = name;
+ }
+ }
+
+ /**
+ * Sets the name of an object with a channel number.
+ *
+ * @param sendable object
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ public static synchronized void setName(Sendable sendable, String moduleType, int channel) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.setName(moduleType, channel);
+ }
+ }
+
+ /**
+ * Sets the name of an object with a module and channel number.
+ *
+ * @param sendable object
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into
+ */
+ public static synchronized void setName(Sendable sendable, String moduleType, int moduleNumber,
+ int channel) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.setName(moduleType, moduleNumber, channel);
+ }
+ }
+
+ /**
+ * Sets both the subsystem name and device name of an object.
+ *
+ * @param sendable object
+ * @param subsystem subsystem name
+ * @param name device name
+ */
+ public static synchronized void setName(Sendable sendable, String subsystem, String name) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.m_name = name;
+ comp.m_subsystem = subsystem;
+ }
+ }
+
+ /**
+ * Gets the subsystem name of an object.
+ *
+ * @param sendable object
+ * @return Subsystem name (empty if object is not in registry)
+ */
+ public static synchronized String getSubsystem(Sendable sendable) {
+ Component comp = components.get(sendable);
+ if (comp == null) {
+ return "";
+ }
+ return comp.m_subsystem;
+ }
+
+ /**
+ * Sets the subsystem name of an object.
+ *
+ * @param sendable object
+ * @param subsystem subsystem name
+ */
+ public static synchronized void setSubsystem(Sendable sendable, String subsystem) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.m_subsystem = subsystem;
+ }
+ }
+
+ /**
+ * Gets a unique handle for setting/getting data with setData() and getData().
+ *
+ * @return Handle
+ */
+ public static synchronized int getDataHandle() {
+ return nextDataHandle++;
+ }
+
+ /**
+ * Associates arbitrary data with an object in the registry.
+ *
+ * @param sendable object
+ * @param handle data handle returned by getDataHandle()
+ * @param data data to set
+ * @return Previous data (may be null)
+ */
+ public static synchronized Object setData(Sendable sendable, int handle, Object data) {
+ Component comp = components.get(sendable);
+ if (comp == null) {
+ return null;
+ }
+ Object rv = null;
+ if (handle < comp.m_data.length) {
+ rv = comp.m_data[handle];
+ } else if (comp.m_data == null) {
+ comp.m_data = new Object[handle + 1];
+ } else {
+ comp.m_data = Arrays.copyOf(comp.m_data, handle + 1);
+ }
+ comp.m_data[handle] = data;
+ return rv;
+ }
+
+ /**
+ * Gets arbitrary data associated with an object in the registry.
+ *
+ * @param sendable object
+ * @param handle data handle returned by getDataHandle()
+ * @return data (may be null if none associated)
+ */
+ public static synchronized Object getData(Sendable sendable, int handle) {
+ Component comp = components.get(sendable);
+ if (comp == null || comp.m_data == null || handle >= comp.m_data.length) {
+ return null;
+ }
+ return comp.m_data[handle];
+ }
+
+ /**
+ * Enables LiveWindow for an object.
+ *
+ * @param sendable object
+ */
+ public static synchronized void enableLiveWindow(Sendable sendable) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.m_liveWindow = true;
+ }
+ }
+
+ /**
+ * Disables LiveWindow for an object.
+ *
+ * @param sendable object
+ */
+ public static synchronized void disableLiveWindow(Sendable sendable) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.m_liveWindow = false;
+ }
+ }
+
+ /**
+ * Publishes an object in the registry to a network table.
+ *
+ * @param sendable object
+ * @param table network table
+ */
+ public static synchronized void publish(Sendable sendable, NetworkTable table) {
+ Component comp = getOrAdd(sendable);
+ comp.m_builder.clearProperties();
+ comp.m_builder.setTable(table);
+ sendable.initSendable(comp.m_builder);
+ comp.m_builder.updateTable();
+ comp.m_builder.startListeners();
+ }
+
+ /**
+ * Updates network table information from an object.
+ *
+ * @param sendable object
+ */
+ public static synchronized void update(Sendable sendable) {
+ Component comp = components.get(sendable);
+ if (comp != null) {
+ comp.m_builder.updateTable();
+ }
+ }
+
+ /**
+ * Data passed to foreachLiveWindow() callback function.
+ */
+ public static class CallbackData {
+ /**
+ * Sendable object.
+ */
+ @SuppressWarnings("MemberName")
+ public Sendable sendable;
+
+ /**
+ * Name.
+ */
+ @SuppressWarnings("MemberName")
+ public String name;
+
+ /**
+ * Subsystem.
+ */
+ @SuppressWarnings("MemberName")
+ public String subsystem;
+
+ /**
+ * Parent sendable object.
+ */
+ @SuppressWarnings("MemberName")
+ public Sendable parent;
+
+ /**
+ * Data stored in object with setData(). Update this to change the data.
+ */
+ @SuppressWarnings("MemberName")
+ public Object data;
+
+ /**
+ * Sendable builder for the sendable.
+ */
+ @SuppressWarnings("MemberName")
+ public SendableBuilderImpl builder;
+ }
+
+ // As foreachLiveWindow is single threaded, cache the components it
+ // iterates over to avoid risk of ConcurrentModificationException
+ private static List<Component> foreachComponents = new ArrayList<>();
+
+ /**
+ * Iterates over LiveWindow-enabled objects in the registry.
+ * It is *not* safe to call other SendableRegistry functions from the
+ * callback.
+ *
+ * @param dataHandle data handle to get data object passed to callback
+ * @param callback function to call for each object
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
+ "PMD.AvoidCatchingThrowable"})
+ public static synchronized void foreachLiveWindow(int dataHandle,
+ Consumer<CallbackData> callback) {
+ CallbackData cbdata = new CallbackData();
+ foreachComponents.clear();
+ foreachComponents.addAll(components.values());
+ for (Component comp : foreachComponents) {
+ if (comp.m_sendable == null) {
+ continue;
+ }
+ cbdata.sendable = comp.m_sendable.get();
+ if (cbdata.sendable != null && comp.m_liveWindow) {
+ cbdata.name = comp.m_name;
+ cbdata.subsystem = comp.m_subsystem;
+ if (comp.m_parent != null) {
+ cbdata.parent = comp.m_parent.get();
+ } else {
+ cbdata.parent = null;
+ }
+ if (comp.m_data != null && dataHandle < comp.m_data.length) {
+ cbdata.data = comp.m_data[dataHandle];
+ } else {
+ cbdata.data = null;
+ }
+ cbdata.builder = comp.m_builder;
+ try {
+ callback.accept(cbdata);
+ } catch (Throwable throwable) {
+ Throwable cause = throwable.getCause();
+ if (cause != null) {
+ throwable = cause;
+ }
+ DriverStation.reportError(
+ "Unhandled exception calling LiveWindow for " + comp.m_name + ": "
+ + throwable.toString(), false);
+ comp.m_liveWindow = false;
+ }
+ if (cbdata.data != null) {
+ if (comp.m_data == null) {
+ comp.m_data = new Object[dataHandle + 1];
+ } else if (dataHandle >= comp.m_data.length) {
+ comp.m_data = Arrays.copyOf(comp.m_data, dataHandle + 1);
+ }
+ comp.m_data[dataHandle] = cbdata.data;
+ }
+ }
+ }
+ foreachComponents.clear();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
new file mode 100644
index 0000000..457ae46
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
@@ -0,0 +1,537 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.nio.ByteBuffer;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Set;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on
+ * the laptop.
+ *
+ * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the
+ * laptop. Users can put values into and get values from the SmartDashboard.
+ */
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+public final class SmartDashboard {
+ /**
+ * The {@link NetworkTable} used by {@link SmartDashboard}.
+ */
+ private static final NetworkTable table =
+ NetworkTableInstance.getDefault().getTable("SmartDashboard");
+
+ /**
+ * A table linking tables in the SmartDashboard to the {@link Sendable} objects they
+ * came from.
+ */
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private static final Map<String, Sendable> tablesToData = new HashMap<>();
+
+ /**
+ * The executor for listener tasks; calls listener tasks synchronously from main thread.
+ */
+ private static final ListenerExecutor listenerExecutor = new ListenerExecutor();
+
+ static {
+ HAL.report(tResourceType.kResourceType_SmartDashboard, 0);
+ }
+
+ private SmartDashboard() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Maps the specified key to the specified value in this table. The key can not be null. The value
+ * can be retrieved by calling the get method with a key that is equal to the original key.
+ *
+ * @param key the key
+ * @param data the value
+ * @throws IllegalArgumentException If key is null
+ */
+ @SuppressWarnings("PMD.CompareObjectsWithEquals")
+ public static synchronized void putData(String key, Sendable data) {
+ Sendable sddata = tablesToData.get(key);
+ if (sddata == null || sddata != data) {
+ tablesToData.put(key, data);
+ NetworkTable dataTable = table.getSubTable(key);
+ SendableRegistry.publish(data, dataTable);
+ dataTable.getEntry(".name").setString(key);
+ }
+ }
+
+ /**
+ * Maps the specified key (where the key is the name of the {@link Sendable}
+ * to the specified value in this table. The value can be retrieved by
+ * calling the get method with a key that is equal to the original key.
+ *
+ * @param value the value
+ * @throws IllegalArgumentException If key is null
+ */
+ public static void putData(Sendable value) {
+ String name = SendableRegistry.getName(value);
+ if (!name.isEmpty()) {
+ putData(name, value);
+ }
+ }
+
+ /**
+ * Returns the value at the specified key.
+ *
+ * @param key the key
+ * @return the value
+ * @throws IllegalArgumentException if the key is null
+ */
+ public static synchronized Sendable getData(String key) {
+ Sendable data = tablesToData.get(key);
+ if (data == null) {
+ throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
+ } else {
+ return data;
+ }
+ }
+
+ /**
+ * Gets the entry for the specified key.
+ * @param key the key name
+ * @return Network table entry.
+ */
+ public static NetworkTableEntry getEntry(String key) {
+ return table.getEntry(key);
+ }
+
+ /**
+ * Checks the table and tells if it contains the specified key.
+ *
+ * @param key the key to search for
+ * @return true if the table as a value assigned to the given key
+ */
+ public static boolean containsKey(String key) {
+ return table.containsKey(key);
+ }
+
+ /**
+ * Get the keys stored in the SmartDashboard table of NetworkTables.
+ *
+ * @param types bitmask of types; 0 is treated as a "don't care".
+ * @return keys currently in the table
+ */
+ public static Set<String> getKeys(int types) {
+ return table.getKeys(types);
+ }
+
+ /**
+ * Get the keys stored in the SmartDashboard table of NetworkTables.
+ *
+ * @return keys currently in the table.
+ */
+ public static Set<String> getKeys() {
+ return table.getKeys();
+ }
+
+ /**
+ * Makes a key's value persistent through program restarts.
+ * The key cannot be null.
+ *
+ * @param key the key name
+ */
+ public static void setPersistent(String key) {
+ getEntry(key).setPersistent();
+ }
+
+ /**
+ * Stop making a key's value persistent through program restarts.
+ * The key cannot be null.
+ *
+ * @param key the key name
+ */
+ public static void clearPersistent(String key) {
+ getEntry(key).clearPersistent();
+ }
+
+ /**
+ * Returns whether the value is persistent through program restarts.
+ * The key cannot be null.
+ *
+ * @param key the key name
+ * @return True if the value is persistent.
+ */
+ public static boolean isPersistent(String key) {
+ return getEntry(key).isPersistent();
+ }
+
+ /**
+ * Sets flags on the specified key in this table. The key can
+ * not be null.
+ *
+ * @param key the key name
+ * @param flags the flags to set (bitmask)
+ */
+ public static void setFlags(String key, int flags) {
+ getEntry(key).setFlags(flags);
+ }
+
+ /**
+ * Clears flags on the specified key in this table. The key can
+ * not be null.
+ *
+ * @param key the key name
+ * @param flags the flags to clear (bitmask)
+ */
+ public static void clearFlags(String key, int flags) {
+ getEntry(key).clearFlags(flags);
+ }
+
+ /**
+ * Returns the flags for the specified key.
+ *
+ * @param key the key name
+ * @return the flags, or 0 if the key is not defined
+ */
+ public static int getFlags(String key) {
+ return getEntry(key).getFlags();
+ }
+
+ /**
+ * Deletes the specified key in this table. The key can
+ * not be null.
+ *
+ * @param key the key name
+ */
+ public static void delete(String key) {
+ table.delete(key);
+ }
+
+ /**
+ * Put a boolean in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putBoolean(String key, boolean value) {
+ return getEntry(key).setBoolean(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultBoolean(String key, boolean defaultValue) {
+ return getEntry(key).setDefaultBoolean(defaultValue);
+ }
+
+ /**
+ * Returns the boolean the key maps to. If the key does not exist or is of
+ * different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static boolean getBoolean(String key, boolean defaultValue) {
+ return getEntry(key).getBoolean(defaultValue);
+ }
+
+ /**
+ * Put a number in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putNumber(String key, double value) {
+ return getEntry(key).setDouble(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultNumber(String key, double defaultValue) {
+ return getEntry(key).setDefaultDouble(defaultValue);
+ }
+
+ /**
+ * Returns the number the key maps to. If the key does not exist or is of
+ * different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static double getNumber(String key, double defaultValue) {
+ return getEntry(key).getDouble(defaultValue);
+ }
+
+ /**
+ * Put a string in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putString(String key, String value) {
+ return getEntry(key).setString(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultString(String key, String defaultValue) {
+ return getEntry(key).setDefaultString(defaultValue);
+ }
+
+ /**
+ * Returns the string the key maps to. If the key does not exist or is of
+ * different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static String getString(String key, String defaultValue) {
+ return getEntry(key).getString(defaultValue);
+ }
+
+ /**
+ * Put a boolean array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putBooleanArray(String key, boolean[] value) {
+ return getEntry(key).setBooleanArray(value);
+ }
+
+ /**
+ * Put a boolean array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putBooleanArray(String key, Boolean[] value) {
+ return getEntry(key).setBooleanArray(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultBooleanArray(String key, boolean[] defaultValue) {
+ return getEntry(key).setDefaultBooleanArray(defaultValue);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultBooleanArray(String key, Boolean[] defaultValue) {
+ return getEntry(key).setDefaultBooleanArray(defaultValue);
+ }
+
+ /**
+ * Returns the boolean array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static boolean[] getBooleanArray(String key, boolean[] defaultValue) {
+ return getEntry(key).getBooleanArray(defaultValue);
+ }
+
+ /**
+ * Returns the boolean array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static Boolean[] getBooleanArray(String key, Boolean[] defaultValue) {
+ return getEntry(key).getBooleanArray(defaultValue);
+ }
+
+ /**
+ * Put a number array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putNumberArray(String key, double[] value) {
+ return getEntry(key).setDoubleArray(value);
+ }
+
+ /**
+ * Put a number array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putNumberArray(String key, Double[] value) {
+ return getEntry(key).setNumberArray(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultNumberArray(String key, double[] defaultValue) {
+ return getEntry(key).setDefaultDoubleArray(defaultValue);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultNumberArray(String key, Double[] defaultValue) {
+ return getEntry(key).setDefaultNumberArray(defaultValue);
+ }
+
+ /**
+ * Returns the number array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static double[] getNumberArray(String key, double[] defaultValue) {
+ return getEntry(key).getDoubleArray(defaultValue);
+ }
+
+ /**
+ * Returns the number array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static Double[] getNumberArray(String key, Double[] defaultValue) {
+ return getEntry(key).getDoubleArray(defaultValue);
+ }
+
+ /**
+ * Put a string array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putStringArray(String key, String[] value) {
+ return getEntry(key).setStringArray(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultStringArray(String key, String[] defaultValue) {
+ return getEntry(key).setDefaultStringArray(defaultValue);
+ }
+
+ /**
+ * Returns the string array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static String[] getStringArray(String key, String[] defaultValue) {
+ return getEntry(key).getStringArray(defaultValue);
+ }
+
+ /**
+ * Put a raw value (byte array) in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putRaw(String key, byte[] value) {
+ return getEntry(key).setRaw(value);
+ }
+
+ /**
+ * Put a raw value (bytes from a byte buffer) in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @param len the length of the value
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putRaw(String key, ByteBuffer value, int len) {
+ return getEntry(key).setRaw(value, len);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultRaw(String key, byte[] defaultValue) {
+ return getEntry(key).setDefaultRaw(defaultValue);
+ }
+
+ /**
+ * Returns the raw value (byte array) the key maps to. If the key does not
+ * exist or is of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static byte[] getRaw(String key, byte[] defaultValue) {
+ return getEntry(key).getRaw(defaultValue);
+ }
+
+ /**
+ * Posts a task from a listener to the ListenerExecutor, so that it can be run synchronously
+ * from the main loop on the next call to {@link SmartDashboard#updateValues()}.
+ *
+ * @param task The task to run synchronously from the main thread.
+ */
+ public static void postListenerTask(Runnable task) {
+ listenerExecutor.execute(task);
+ }
+
+ /**
+ * Puts all sendable data to the dashboard.
+ */
+ public static synchronized void updateValues() {
+ for (Sendable data : tablesToData.values()) {
+ SendableRegistry.update(data);
+ }
+ // Execute posted listener tasks
+ listenerExecutor.runListenerTasks();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
new file mode 100644
index 0000000..578787c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class CubicHermiteSpline extends Spline {
+ private static SimpleMatrix hermiteBasis;
+ private final SimpleMatrix m_coefficients;
+
+ /**
+ * Constructs a cubic hermite spline with the specified control vectors. Each
+ * control vector contains info about the location of the point and its first
+ * derivative.
+ *
+ * @param xInitialControlVector The control vector for the initial point in
+ * the x dimension.
+ * @param xFinalControlVector The control vector for the final point in
+ * the x dimension.
+ * @param yInitialControlVector The control vector for the initial point in
+ * the y dimension.
+ * @param yFinalControlVector The control vector for the final point in
+ * the y dimension.
+ */
+ @SuppressWarnings("ParameterName")
+ public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
+ double[] yInitialControlVector, double[] yFinalControlVector) {
+ super(3);
+
+ // Populate the coefficients for the actual spline equations.
+ // Row 0 is x coefficients
+ // Row 1 is y coefficients
+ final var hermite = makeHermiteBasis();
+ final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+ final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+ final var xCoeffs = (hermite.mult(x)).transpose();
+ final var yCoeffs = (hermite.mult(y)).transpose();
+
+ m_coefficients = new SimpleMatrix(6, 4);
+
+ for (int i = 0; i < 4; i++) {
+ m_coefficients.set(0, i, xCoeffs.get(0, i));
+ m_coefficients.set(1, i, yCoeffs.get(0, i));
+
+ // Populate Row 2 and Row 3 with the derivatives of the equations above.
+ // Then populate row 4 and 5 with the second derivatives.
+ // Here, we are multiplying by (3 - i) to manually take the derivative. The
+ // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
+ m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
+ m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
+ }
+
+ for (int i = 0; i < 3; i++) {
+ // Here, we are multiplying by (2 - i) to manually take the derivative. The
+ // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
+ m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
+ m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
+ }
+
+ }
+
+ /**
+ * Returns the coefficients matrix.
+ *
+ * @return The coefficients matrix.
+ */
+ @Override
+ protected SimpleMatrix getCoefficients() {
+ return m_coefficients;
+ }
+
+ /**
+ * Returns the hermite basis matrix for cubic hermite spline interpolation.
+ *
+ * @return The hermite basis matrix for cubic hermite spline interpolation.
+ */
+ private SimpleMatrix makeHermiteBasis() {
+ if (hermiteBasis == null) {
+ hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{
+ +2.0, +1.0, -2.0, +1.0,
+ -3.0, -2.0, +3.0, -1.0,
+ +0.0, +1.0, +0.0, +0.0,
+ +1.0, +0.0, +0.0, +0.0
+ });
+ }
+ return hermiteBasis;
+ }
+
+ /**
+ * Returns the control vector for each dimension as a matrix from the
+ * user-provided arrays in the constructor.
+ *
+ * @param initialVector The control vector for the initial point.
+ * @param finalVector The control vector for the final point.
+ * @return The control vector matrix for a dimension.
+ */
+ private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+ if (initialVector.length != 2 || finalVector.length != 2) {
+ throw new IllegalArgumentException("Size of vectors must be 2");
+ }
+ return new SimpleMatrix(4, 1, true, new double[]{
+ initialVector[0], initialVector[1],
+ finalVector[0], finalVector[1]});
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
new file mode 100644
index 0000000..62c1e5a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * Represents a pair of a pose and a curvature.
+ */
+@SuppressWarnings("MemberName")
+public class PoseWithCurvature {
+ // Represents the pose.
+ public Pose2d poseMeters;
+
+ // Represents the curvature.
+ public double curvatureRadPerMeter;
+
+ /**
+ * Constructs a PoseWithCurvature.
+ *
+ * @param poseMeters The pose.
+ * @param curvatureRadPerMeter The curvature.
+ */
+ public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
+ this.poseMeters = poseMeters;
+ this.curvatureRadPerMeter = curvatureRadPerMeter;
+ }
+
+ /**
+ * Constructs a PoseWithCurvature with default values.
+ */
+ public PoseWithCurvature() {
+ poseMeters = new Pose2d();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
new file mode 100644
index 0000000..3b2d3eb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class QuinticHermiteSpline extends Spline {
+ private static SimpleMatrix hermiteBasis;
+ private final SimpleMatrix m_coefficients;
+
+ /**
+ * Constructs a quintic hermite spline with the specified control vectors.
+ * Each control vector contains into about the location of the point, its
+ * first derivative, and its second derivative.
+ *
+ * @param xInitialControlVector The control vector for the initial point in
+ * the x dimension.
+ * @param xFinalControlVector The control vector for the final point in
+ * the x dimension.
+ * @param yInitialControlVector The control vector for the initial point in
+ * the y dimension.
+ * @param yFinalControlVector The control vector for the final point in
+ * the y dimension.
+ */
+ @SuppressWarnings("ParameterName")
+ public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
+ double[] yInitialControlVector, double[] yFinalControlVector) {
+ super(5);
+
+ // Populate the coefficients for the actual spline equations.
+ // Row 0 is x coefficients
+ // Row 1 is y coefficients
+ final var hermite = makeHermiteBasis();
+ final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+ final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+ final var xCoeffs = (hermite.mult(x)).transpose();
+ final var yCoeffs = (hermite.mult(y)).transpose();
+
+ m_coefficients = new SimpleMatrix(6, 6);
+
+ for (int i = 0; i < 6; i++) {
+ m_coefficients.set(0, i, xCoeffs.get(0, i));
+ m_coefficients.set(1, i, yCoeffs.get(0, i));
+ }
+ for (int i = 0; i < 6; i++) {
+ // Populate Row 2 and Row 3 with the derivatives of the equations above.
+ // Here, we are multiplying by (5 - i) to manually take the derivative. The
+ // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
+ m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
+ m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
+ }
+ for (int i = 0; i < 5; i++) {
+ // Then populate row 4 and 5 with the second derivatives.
+ // Here, we are multiplying by (4 - i) to manually take the derivative. The
+ // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
+ m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
+ m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
+ }
+ }
+
+ /**
+ * Returns the coefficients matrix.
+ *
+ * @return The coefficients matrix.
+ */
+ @Override
+ protected SimpleMatrix getCoefficients() {
+ return m_coefficients;
+ }
+
+ /**
+ * Returns the hermite basis matrix for quintic hermite spline interpolation.
+ *
+ * @return The hermite basis matrix for quintic hermite spline interpolation.
+ */
+ private SimpleMatrix makeHermiteBasis() {
+ if (hermiteBasis == null) {
+ hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{
+ -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+ +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+ -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+ +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+ +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+ +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
+ });
+ }
+ return hermiteBasis;
+ }
+
+ /**
+ * Returns the control vector for each dimension as a matrix from the
+ * user-provided arrays in the constructor.
+ *
+ * @param initialVector The control vector for the initial point.
+ * @param finalVector The control vector for the final point.
+ * @return The control vector matrix for a dimension.
+ */
+ private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+ if (initialVector.length != 3 || finalVector.length != 3) {
+ throw new IllegalArgumentException("Size of vectors must be 3");
+ }
+ return new SimpleMatrix(6, 1, true, new double[]{
+ initialVector[0], initialVector[1], initialVector[2],
+ finalVector[0], finalVector[1], finalVector[2]});
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
new file mode 100644
index 0000000..a0dfd19
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.Arrays;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents a two-dimensional parametric spline that interpolates between two
+ * points.
+ */
+public abstract class Spline {
+ private final int m_degree;
+
+ /**
+ * Constructs a spline with the given degree.
+ *
+ * @param degree The degree of the spline.
+ */
+ Spline(int degree) {
+ m_degree = degree;
+ }
+
+ /**
+ * Returns the coefficients of the spline.
+ *
+ * @return The coefficients of the spline.
+ */
+ protected abstract SimpleMatrix getCoefficients();
+
+ /**
+ * Gets the pose and curvature at some point t on the spline.
+ *
+ * @param t The point t
+ * @return The pose and curvature at that point.
+ */
+ @SuppressWarnings("ParameterName")
+ public PoseWithCurvature getPoint(double t) {
+ SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
+ final var coefficients = getCoefficients();
+
+ // Populate the polynomial bases.
+ for (int i = 0; i <= m_degree; i++) {
+ polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
+ }
+
+ // This simply multiplies by the coefficients. We need to divide out t some
+ // n number of times where n is the derivative we want to take.
+ SimpleMatrix combined = coefficients.mult(polynomialBases);
+
+ // Get x and y
+ final double x = combined.get(0, 0);
+ final double y = combined.get(1, 0);
+
+ double dx;
+ double dy;
+ double ddx;
+ double ddy;
+
+ if (t == 0) {
+ dx = coefficients.get(2, m_degree - 1);
+ dy = coefficients.get(3, m_degree - 1);
+ ddx = coefficients.get(4, m_degree - 2);
+ ddy = coefficients.get(5, m_degree - 2);
+ } else {
+ // Divide out t once for first derivative.
+ dx = combined.get(2, 0) / t;
+ dy = combined.get(3, 0) / t;
+
+ // Divide out t twice for second derivative.
+ ddx = combined.get(4, 0) / t / t;
+ ddy = combined.get(5, 0) / t / t;
+ }
+
+ // Find the curvature.
+ final double curvature =
+ (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
+
+ return new PoseWithCurvature(
+ new Pose2d(x, y, new Rotation2d(dx, dy)),
+ curvature
+ );
+ }
+
+ /**
+ * Represents a control vector for a spline.
+ *
+ * <p>Each element in each array represents the value of the derivative at the index. For
+ * example, the value of x[2] is the second derivative in the x dimension.
+ */
+ @SuppressWarnings("MemberName")
+ public static class ControlVector {
+ public double[] x;
+ public double[] y;
+
+ /**
+ * Instantiates a control vector.
+ * @param x The x dimension of the control vector.
+ * @param y The y dimension of the control vector.
+ */
+ @SuppressWarnings("ParameterName")
+ public ControlVector(double[] x, double[] y) {
+ this.x = Arrays.copyOf(x, x.length);
+ this.y = Arrays.copyOf(y, y.length);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
new file mode 100644
index 0000000..a419bf9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
@@ -0,0 +1,276 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+public final class SplineHelper {
+ /**
+ * Private constructor because this is a utility class.
+ */
+ private SplineHelper() {
+ }
+
+ /**
+ * Returns 2 cubic control vectors from a set of exterior waypoints and
+ * interior translations.
+ *
+ * @param start The starting pose.
+ * @param interiorWaypoints The interior waypoints.
+ * @param end The ending pose.
+ * @return 2 cubic control vectors.
+ */
+ public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
+ Pose2d start, Translation2d[] interiorWaypoints, Pose2d end
+ ) {
+ // Generate control vectors from poses.
+ Spline.ControlVector initialCV;
+ Spline.ControlVector endCV;
+
+ // Chooses a magnitude automatically that makes the splines look better.
+ if (interiorWaypoints.length < 1) {
+ double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
+ initialCV = getCubicControlVector(scalar, start);
+ endCV = getCubicControlVector(scalar, end);
+ } else {
+ double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
+ initialCV = getCubicControlVector(scalar, start);
+ scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1])
+ * 1.2;
+ endCV = getCubicControlVector(scalar, end);
+ }
+ return new Spline.ControlVector[]{initialCV, endCV};
+ }
+
+ /**
+ * Returns quintic control vectors from a set of waypoints.
+ *
+ * @param waypoints The waypoints
+ * @return List of control vectors
+ */
+ public static List<Spline.ControlVector> getQuinticControlVectorsFromWaypoints(
+ List<Pose2d> waypoints
+ ) {
+ List<Spline.ControlVector> vectors = new ArrayList<>();
+ for (int i = 0; i < waypoints.size() - 1; i++) {
+ var p0 = waypoints.get(i);
+ var p1 = waypoints.get(i + 1);
+
+ // This just makes the splines look better.
+ final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
+
+ vectors.add(getQuinticControlVector(scalar, p0));
+ vectors.add(getQuinticControlVector(scalar, p1));
+ }
+ return vectors;
+ }
+
+ /**
+ * Returns a set of cubic splines corresponding to the provided control vectors. The
+ * user is free to set the direction of the start and end point. The
+ * directions for the middle waypoints are determined automatically to ensure
+ * continuous curvature throughout the path.
+ *
+ * @param start The starting control vector.
+ * @param waypoints The middle waypoints. This can be left blank if you only
+ * wish to create a path with two waypoints.
+ * @param end The ending control vector.
+ * @return A vector of cubic hermite splines that interpolate through the
+ * provided waypoints and control vectors.
+ */
+ @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
+ "PMD.AvoidInstantiatingObjectsInLoops"})
+ public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
+ Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
+
+ CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
+
+ double[] xInitial = start.x;
+ double[] yInitial = start.y;
+ double[] xFinal = end.x;
+ double[] yFinal = end.y;
+
+ if (waypoints.length > 1) {
+ Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
+
+ // Create an array of all waypoints, including the start and end.
+ newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
+ System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
+ newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
+
+ // Populate tridiagonal system for clamped cubic
+ /* See:
+ https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+ /undervisningsmateriale/chap7alecture.pdf
+ */
+ // Above-diagonal of tridiagonal matrix, zero-padded
+ final double[] a = new double[newWaypts.length - 2];
+
+ // Diagonal of tridiagonal matrix
+ final double[] b = new double[newWaypts.length - 2];
+ Arrays.fill(b, 4.0);
+
+ // Below-diagonal of tridiagonal matrix, zero-padded
+ final double[] c = new double[newWaypts.length - 2];
+
+ // rhs vectors
+ final double[] dx = new double[newWaypts.length - 2];
+ final double[] dy = new double[newWaypts.length - 2];
+
+ // solution vectors
+ final double[] fx = new double[newWaypts.length - 2];
+ final double[] fy = new double[newWaypts.length - 2];
+
+ // populate above-diagonal and below-diagonal vectors
+ a[0] = 0.0;
+ for (int i = 0; i < newWaypts.length - 3; i++) {
+ a[i + 1] = 1;
+ c[i] = 1;
+ }
+ c[c.length - 1] = 0.0;
+
+ // populate rhs vectors
+ dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
+ dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
+
+ if (newWaypts.length > 4) {
+ for (int i = 1; i <= newWaypts.length - 4; i++) {
+ dx[i] = 3 * (newWaypts[i + 1].getX() - newWaypts[i - 1].getX());
+ dy[i] = 3 * (newWaypts[i + 1].getY() - newWaypts[i - 1].getY());
+ }
+ }
+
+ dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
+ - newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
+ dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
+ - newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
+
+ // Compute solution to tridiagonal system
+ thomasAlgorithm(a, b, c, dx, fx);
+ thomasAlgorithm(a, b, c, dy, fy);
+
+ double[] newFx = new double[fx.length + 2];
+ double[] newFy = new double[fy.length + 2];
+
+ newFx[0] = xInitial[1];
+ newFy[0] = yInitial[1];
+ System.arraycopy(fx, 0, newFx, 1, fx.length);
+ System.arraycopy(fy, 0, newFy, 1, fy.length);
+ newFx[newFx.length - 1] = xFinal[1];
+ newFy[newFy.length - 1] = yFinal[1];
+
+ for (int i = 0; i < newFx.length - 1; i++) {
+ splines[i] = new CubicHermiteSpline(
+ new double[]{newWaypts[i].getX(), newFx[i]},
+ new double[]{newWaypts[i + 1].getX(), newFx[i + 1]},
+ new double[]{newWaypts[i].getY(), newFy[i]},
+ new double[]{newWaypts[i + 1].getY(), newFy[i + 1]}
+ );
+ }
+ } else if (waypoints.length == 1) {
+ final var xDeriv = (3 * (xFinal[0]
+ - xInitial[0])
+ - xFinal[1] - xInitial[1])
+ / 4.0;
+ final var yDeriv = (3 * (yFinal[0]
+ - yInitial[0])
+ - yFinal[1] - yInitial[1])
+ / 4.0;
+
+ double[] midXControlVector = {waypoints[0].getX(), xDeriv};
+ double[] midYControlVector = {waypoints[0].getY(), yDeriv};
+
+ splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
+ yInitial, midYControlVector);
+ splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
+ midYControlVector, yFinal);
+ } else {
+ splines[0] = new CubicHermiteSpline(xInitial, xFinal,
+ yInitial, yFinal);
+ }
+ return splines;
+ }
+
+ /**
+ * Returns a set of quintic splines corresponding to the provided control vectors.
+ * The user is free to set the direction of all control vectors. Continuous
+ * curvature is guaranteed throughout the path.
+ *
+ * @param controlVectors The control vectors.
+ * @return A vector of quintic hermite splines that interpolate through the
+ * provided waypoints.
+ */
+ @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+ public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
+ Spline.ControlVector[] controlVectors) {
+ QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+ for (int i = 0; i < controlVectors.length - 1; i++) {
+ var xInitial = controlVectors[i].x;
+ var xFinal = controlVectors[i + 1].x;
+ var yInitial = controlVectors[i].y;
+ var yFinal = controlVectors[i + 1].y;
+ splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
+ yInitial, yFinal);
+ }
+ return splines;
+ }
+
+ /**
+ * Thomas algorithm for solving tridiagonal systems Af = d.
+ *
+ * @param a the values of A above the diagonal
+ * @param b the values of A on the diagonal
+ * @param c the values of A below the diagonal
+ * @param d the vector on the rhs
+ * @param solutionVector the unknown (solution) vector, modified in-place
+ */
+ @SuppressWarnings({"ParameterName", "LocalVariableName"})
+ private static void thomasAlgorithm(double[] a, double[] b,
+ double[] c, double[] d, double[] solutionVector) {
+ int N = d.length;
+
+ double[] cStar = new double[N];
+ double[] dStar = new double[N];
+
+ // This updates the coefficients in the first row
+ // Note that we should be checking for division by zero here
+ cStar[0] = c[0] / b[0];
+ dStar[0] = d[0] / b[0];
+
+ // Create the c_star and d_star coefficients in the forward sweep
+ for (int i = 1; i < N; i++) {
+ double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
+ cStar[i] = c[i] * m;
+ dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
+ }
+ solutionVector[N - 1] = dStar[N - 1];
+ // This is the reverse sweep, used to update the solution vector f
+ for (int i = N - 2; i >= 0; i--) {
+ solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
+ }
+ }
+
+ private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
+ return new Spline.ControlVector(
+ new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos()},
+ new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()}
+ );
+ }
+
+ private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
+ return new Spline.ControlVector(
+ new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0},
+ new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0}
+ );
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
new file mode 100644
index 0000000..4b7bf95
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+public final class SplineParameterizer {
+ private static final double kMaxDx = 0.127;
+ private static final double kMaxDy = 0.00127;
+ private static final double kMaxDtheta = 0.0872;
+
+ /**
+ * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
+ * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
+ * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
+ * this maximum should definitely indicate something has gone wrong.
+ */
+ private static final int kMaxIterations = 5000;
+
+ @SuppressWarnings("MemberName")
+ private static class StackContents {
+ final double t1;
+ final double t0;
+
+ StackContents(double t0, double t1) {
+ this.t0 = t0;
+ this.t1 = t1;
+ }
+ }
+
+ public static class MalformedSplineException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ private MalformedSplineException(String message) {
+ super(message);
+ }
+ }
+
+ /**
+ * Private constructor because this is a utility class.
+ */
+ private SplineParameterizer() {
+ }
+
+ /**
+ * Parameterizes the spline. This method breaks up the spline into various
+ * arcs until their dx, dy, and dtheta are within specific tolerances.
+ *
+ * @param spline The spline to parameterize.
+ * @return A list of poses and curvatures that represents various points on the spline.
+ * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+ * with approximately opposing headings)
+ */
+ public static List<PoseWithCurvature> parameterize(Spline spline) {
+ return parameterize(spline, 0.0, 1.0);
+ }
+
+ /**
+ * Parameterizes the spline. This method breaks up the spline into various
+ * arcs until their dx, dy, and dtheta are within specific tolerances.
+ *
+ * @param spline The spline to parameterize.
+ * @param t0 Starting internal spline parameter. It is recommended to use 0.0.
+ * @param t1 Ending internal spline parameter. It is recommended to use 1.0.
+ * @return A list of poses and curvatures that represents various points on the spline.
+ * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+ * with approximately opposing headings)
+ */
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
+ var splinePoints = new ArrayList<PoseWithCurvature>();
+
+ // The parameterization does not add the initial point. Let's add that.
+ splinePoints.add(spline.getPoint(t0));
+
+ // We use an "explicit stack" to simulate recursion, instead of a recursive function call
+ // This give us greater control, instead of a stack overflow
+ var stack = new ArrayDeque<StackContents>();
+ stack.push(new StackContents(t0, t1));
+
+ StackContents current;
+ PoseWithCurvature start;
+ PoseWithCurvature end;
+ int iterations = 0;
+
+ while (!stack.isEmpty()) {
+ current = stack.removeFirst();
+ start = spline.getPoint(current.t0);
+ end = spline.getPoint(current.t1);
+
+ final var twist = start.poseMeters.log(end.poseMeters);
+ if (
+ Math.abs(twist.dy) > kMaxDy
+ || Math.abs(twist.dx) > kMaxDx
+ || Math.abs(twist.dtheta) > kMaxDtheta
+ ) {
+ stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+ stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+ } else {
+ splinePoints.add(spline.getPoint(current.t1));
+ }
+
+ iterations++;
+ if (iterations >= kMaxIterations) {
+ throw new MalformedSplineException(
+ "Could not parameterize a malformed spline. "
+ + "This means that you probably had two or more adjacent waypoints that were very close "
+ + "together with headings in opposing directions."
+ );
+ }
+ }
+
+ return splinePoints;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
new file mode 100644
index 0000000..f1d51b9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
@@ -0,0 +1,331 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+public class Trajectory {
+ private final double m_totalTimeSeconds;
+ private final List<State> m_states;
+
+ /**
+ * Constructs a trajectory from a vector of states.
+ *
+ * @param states A vector of states.
+ */
+ public Trajectory(final List<State> states) {
+ m_states = states;
+ m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
+ }
+
+ /**
+ * Linearly interpolates between two values.
+ *
+ * @param startValue The start value.
+ * @param endValue The end value.
+ * @param t The fraction for interpolation.
+ * @return The interpolated value.
+ */
+ @SuppressWarnings("ParameterName")
+ private static double lerp(double startValue, double endValue, double t) {
+ return startValue + (endValue - startValue) * t;
+ }
+
+ /**
+ * Linearly interpolates between two poses.
+ *
+ * @param startValue The start pose.
+ * @param endValue The end pose.
+ * @param t The fraction for interpolation.
+ * @return The interpolated pose.
+ */
+ @SuppressWarnings("ParameterName")
+ private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
+ return startValue.plus((endValue.minus(startValue)).times(t));
+ }
+
+ /**
+ * Returns the initial pose of the trajectory.
+ *
+ * @return The initial pose of the trajectory.
+ */
+ public Pose2d getInitialPose() {
+ return sample(0).poseMeters;
+ }
+
+ /**
+ * Returns the overall duration of the trajectory.
+ *
+ * @return The duration of the trajectory.
+ */
+ public double getTotalTimeSeconds() {
+ return m_totalTimeSeconds;
+ }
+
+ /**
+ * Return the states of the trajectory.
+ *
+ * @return The states of the trajectory.
+ */
+ public List<State> getStates() {
+ return m_states;
+ }
+
+ /**
+ * Sample the trajectory at a point in time.
+ *
+ * @param timeSeconds The point in time since the beginning of the trajectory to sample.
+ * @return The state at that point in time.
+ */
+ public State sample(double timeSeconds) {
+ if (timeSeconds <= m_states.get(0).timeSeconds) {
+ return m_states.get(0);
+ }
+ if (timeSeconds >= m_totalTimeSeconds) {
+ return m_states.get(m_states.size() - 1);
+ }
+
+ // To get the element that we want, we will use a binary search algorithm
+ // instead of iterating over a for-loop. A binary search is O(std::log(n))
+ // whereas searching using a loop is O(n).
+
+ // This starts at 1 because we use the previous state later on for
+ // interpolation.
+ int low = 1;
+ int high = m_states.size() - 1;
+
+ while (low != high) {
+ int mid = (low + high) / 2;
+ if (m_states.get(mid).timeSeconds < timeSeconds) {
+ // This index and everything under it are less than the requested
+ // timestamp. Therefore, we can discard them.
+ low = mid + 1;
+ } else {
+ // t is at least as large as the element at this index. This means that
+ // anything after it cannot be what we are looking for.
+ high = mid;
+ }
+ }
+
+ // High and Low should be the same.
+
+ // The sample's timestamp is now greater than or equal to the requested
+ // timestamp. If it is greater, we need to interpolate between the
+ // previous state and the current state to get the exact state that we
+ // want.
+ final State sample = m_states.get(low);
+ final State prevSample = m_states.get(low - 1);
+
+ // If the difference in states is negligible, then we are spot on!
+ if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
+ return sample;
+ }
+ // Interpolate between the two states for the state that we want.
+ return prevSample.interpolate(sample,
+ (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
+ }
+
+ /**
+ * Transforms all poses in the trajectory by the given transform. This is
+ * useful for converting a robot-relative trajectory into a field-relative
+ * trajectory. This works with respect to the first pose in the trajectory.
+ *
+ * @param transform The transform to transform the trajectory by.
+ * @return The transformed trajectory.
+ */
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ public Trajectory transformBy(Transform2d transform) {
+ var firstState = m_states.get(0);
+ var firstPose = firstState.poseMeters;
+
+ // Calculate the transformed first pose.
+ var newFirstPose = firstPose.plus(transform);
+ List<State> newStates = new ArrayList<>();
+
+ newStates.add(new State(
+ firstState.timeSeconds, firstState.velocityMetersPerSecond,
+ firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
+ ));
+
+ for (int i = 1; i < m_states.size(); i++) {
+ var state = m_states.get(i);
+ // We are transforming relative to the coordinate frame of the new initial pose.
+ newStates.add(new State(
+ state.timeSeconds, state.velocityMetersPerSecond,
+ state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
+ state.curvatureRadPerMeter
+ ));
+ }
+
+ return new Trajectory(newStates);
+ }
+
+ /**
+ * Transforms all poses in the trajectory so that they are relative to the
+ * given pose. This is useful for converting a field-relative trajectory
+ * into a robot-relative trajectory.
+ *
+ * @param pose The pose that is the origin of the coordinate frame that
+ * the current trajectory will be transformed into.
+ * @return The transformed trajectory.
+ */
+ public Trajectory relativeTo(Pose2d pose) {
+ return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
+ state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
+ state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
+ .collect(Collectors.toList()));
+ }
+
+ /**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+ @SuppressWarnings("MemberName")
+ public static class State {
+ // The time elapsed since the beginning of the trajectory.
+ @JsonProperty("time")
+ public double timeSeconds;
+
+ // The speed at that point of the trajectory.
+ @JsonProperty("velocity")
+ public double velocityMetersPerSecond;
+
+ // The acceleration at that point of the trajectory.
+ @JsonProperty("acceleration")
+ public double accelerationMetersPerSecondSq;
+
+ // The pose at that point of the trajectory.
+ @JsonProperty("pose")
+ public Pose2d poseMeters;
+
+ // The curvature at that point of the trajectory.
+ @JsonProperty("curvature")
+ public double curvatureRadPerMeter;
+
+ public State() {
+ poseMeters = new Pose2d();
+ }
+
+ /**
+ * Constructs a State with the specified parameters.
+ *
+ * @param timeSeconds The time elapsed since the beginning of the trajectory.
+ * @param velocityMetersPerSecond The speed at that point of the trajectory.
+ * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
+ * @param poseMeters The pose at that point of the trajectory.
+ * @param curvatureRadPerMeter The curvature at that point of the trajectory.
+ */
+ public State(double timeSeconds, double velocityMetersPerSecond,
+ double accelerationMetersPerSecondSq, Pose2d poseMeters,
+ double curvatureRadPerMeter) {
+ this.timeSeconds = timeSeconds;
+ this.velocityMetersPerSecond = velocityMetersPerSecond;
+ this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
+ this.poseMeters = poseMeters;
+ this.curvatureRadPerMeter = curvatureRadPerMeter;
+ }
+
+ /**
+ * Interpolates between two States.
+ *
+ * @param endValue The end value for the interpolation.
+ * @param i The interpolant (fraction).
+ * @return The interpolated state.
+ */
+ @SuppressWarnings("ParameterName")
+ State interpolate(State endValue, double i) {
+ // Find the new t value.
+ final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
+
+ // Find the delta time between the current state and the interpolated state.
+ final double deltaT = newT - timeSeconds;
+
+ // If delta time is negative, flip the order of interpolation.
+ if (deltaT < 0) {
+ return endValue.interpolate(this, 1 - i);
+ }
+
+ // Check whether the robot is reversing at this stage.
+ final boolean reversing = velocityMetersPerSecond < 0
+ || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
+
+ // Calculate the new velocity
+ // v_f = v_0 + at
+ final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
+
+ // Calculate the change in position.
+ // delta_s = v_0 t + 0.5 at^2
+ final double newS = (velocityMetersPerSecond * deltaT
+ + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
+
+ // Return the new state. To find the new position for the new state, we need
+ // to interpolate between the two endpoint poses. The fraction for
+ // interpolation is the change in position (delta s) divided by the total
+ // distance between the two endpoints.
+ final double interpolationFrac = newS
+ / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
+
+ return new State(
+ newT, newV, accelerationMetersPerSecondSq,
+ lerp(poseMeters, endValue.poseMeters, interpolationFrac),
+ lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
+ );
+ }
+
+ @Override
+ public String toString() {
+ return String.format(
+ "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+ timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
+ poseMeters, curvatureRadPerMeter);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof State)) {
+ return false;
+ }
+ State state = (State) obj;
+ return Double.compare(state.timeSeconds, timeSeconds) == 0
+ && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
+ && Double.compare(state.accelerationMetersPerSecondSq,
+ accelerationMetersPerSecondSq) == 0
+ && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
+ && Objects.equals(poseMeters, state.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(timeSeconds, velocityMetersPerSecond,
+ accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
+ }
+ }
+
+ @Override
+ public String toString() {
+ String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+ return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
new file mode 100644
index 0000000..fbc5ddb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+/**
+ * Represents the configuration for generating a trajectory. This class stores the start velocity,
+ * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
+ *
+ * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
+ * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable
+ * values (0, 0, {}, false). These values can be changed via the setXXX methods.
+ */
+public class TrajectoryConfig {
+ private final double m_maxVelocity;
+ private final double m_maxAcceleration;
+ private final List<TrajectoryConstraint> m_constraints;
+ private double m_startVelocity;
+ private double m_endVelocity;
+ private boolean m_reversed;
+
+ /**
+ * Constructs the trajectory configuration class.
+ *
+ * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
+ * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+ */
+ public TrajectoryConfig(double maxVelocityMetersPerSecond,
+ double maxAccelerationMetersPerSecondSq) {
+ m_maxVelocity = maxVelocityMetersPerSecond;
+ m_maxAcceleration = maxAccelerationMetersPerSecondSq;
+ m_constraints = new ArrayList<>();
+ }
+
+ /**
+ * Adds a user-defined constraint to the trajectory.
+ *
+ * @param constraint The user-defined constraint.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
+ m_constraints.add(constraint);
+ return this;
+ }
+
+ /**
+ * Adds all user-defined constraints from a list to the trajectory.
+ * @param constraints List of user-defined constraints.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
+ m_constraints.addAll(constraints);
+ return this;
+ }
+
+ /**
+ * Adds a differential drive kinematics constraint to ensure that
+ * no wheel velocity of a differential drive goes above the max velocity.
+ *
+ * @param kinematics The differential drive kinematics.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
+ addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ return this;
+ }
+
+ /**
+ * Adds a mecanum drive kinematics constraint to ensure that
+ * no wheel velocity of a mecanum drive goes above the max velocity.
+ *
+ * @param kinematics The mecanum drive kinematics.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
+ addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ return this;
+ }
+
+ /**
+ * Adds a swerve drive kinematics constraint to ensure that
+ * no wheel velocity of a swerve drive goes above the max velocity.
+ *
+ * @param kinematics The swerve drive kinematics.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
+ addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ return this;
+ }
+
+ /**
+ * Returns the starting velocity of the trajectory.
+ *
+ * @return The starting velocity of the trajectory.
+ */
+ public double getStartVelocity() {
+ return m_startVelocity;
+ }
+
+ /**
+ * Sets the start velocity of the trajectory.
+ *
+ * @param startVelocityMetersPerSecond The start velocity of the trajectory.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
+ m_startVelocity = startVelocityMetersPerSecond;
+ return this;
+ }
+
+ /**
+ * Returns the starting velocity of the trajectory.
+ *
+ * @return The starting velocity of the trajectory.
+ */
+ public double getEndVelocity() {
+ return m_endVelocity;
+ }
+
+ /**
+ * Sets the end velocity of the trajectory.
+ *
+ * @param endVelocityMetersPerSecond The end velocity of the trajectory.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
+ m_endVelocity = endVelocityMetersPerSecond;
+ return this;
+ }
+
+ /**
+ * Returns the maximum velocity of the trajectory.
+ *
+ * @return The maximum velocity of the trajectory.
+ */
+ public double getMaxVelocity() {
+ return m_maxVelocity;
+ }
+
+ /**
+ * Returns the maximum acceleration of the trajectory.
+ *
+ * @return The maximum acceleration of the trajectory.
+ */
+ public double getMaxAcceleration() {
+ return m_maxAcceleration;
+ }
+
+ /**
+ * Returns the user-defined constraints of the trajectory.
+ *
+ * @return The user-defined constraints of the trajectory.
+ */
+ public List<TrajectoryConstraint> getConstraints() {
+ return m_constraints;
+ }
+
+ /**
+ * Returns whether the trajectory is reversed or not.
+ *
+ * @return whether the trajectory is reversed or not.
+ */
+ public boolean isReversed() {
+ return m_reversed;
+ }
+
+ /**
+ * Sets the reversed flag of the trajectory.
+ *
+ * @param reversed Whether the trajectory should be reversed or not.
+ * @return Instance of the current config object.
+ */
+ public TrajectoryConfig setReversed(boolean reversed) {
+ m_reversed = reversed;
+ return this;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
new file mode 100644
index 0000000..2e2ae7d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -0,0 +1,229 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
+import edu.wpi.first.wpilibj.spline.Spline;
+import edu.wpi.first.wpilibj.spline.SplineHelper;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+public final class TrajectoryGenerator {
+ private static final Trajectory kDoNothingTrajectory =
+ new Trajectory(Arrays.asList(new Trajectory.State()));
+
+ /**
+ * Private constructor because this is a utility class.
+ */
+ private TrajectoryGenerator() {
+ }
+
+ /**
+ * Generates a trajectory from the given control vectors and config. This method uses clamped
+ * cubic splines -- a method in which the exterior control vectors and interior waypoints
+ * are provided. The headings are automatically determined at the interior points to
+ * ensure continuous curvature.
+ *
+ * @param initial The initial control vector.
+ * @param interiorWaypoints The interior waypoints.
+ * @param end The ending control vector.
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ public static Trajectory generateTrajectory(
+ Spline.ControlVector initial,
+ List<Translation2d> interiorWaypoints,
+ Spline.ControlVector end,
+ TrajectoryConfig config
+ ) {
+ final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+ // Clone the control vectors.
+ var newInitial = new Spline.ControlVector(initial.x, initial.y);
+ var newEnd = new Spline.ControlVector(end.x, end.y);
+
+ // Change the orientation if reversed.
+ if (config.isReversed()) {
+ newInitial.x[1] *= -1;
+ newInitial.y[1] *= -1;
+ newEnd.x[1] *= -1;
+ newEnd.y[1] *= -1;
+ }
+
+ // Get the spline points
+ List<PoseWithCurvature> points;
+ try {
+ points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
+ interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+ } catch (MalformedSplineException ex) {
+ DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+ return kDoNothingTrajectory;
+ }
+
+ // Change the points back to their original orientation.
+ if (config.isReversed()) {
+ for (var point : points) {
+ point.poseMeters = point.poseMeters.plus(flip);
+ point.curvatureRadPerMeter *= -1;
+ }
+ }
+
+ // Generate and return trajectory.
+ return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+ config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+ config.getMaxAcceleration(), config.isReversed());
+ }
+
+ /**
+ * Generates a trajectory from the given waypoints and config. This method uses clamped
+ * cubic splines -- a method in which the initial pose, final pose, and interior waypoints
+ * are provided. The headings are automatically determined at the interior points to
+ * ensure continuous curvature.
+ *
+ * @param start The starting pose.
+ * @param interiorWaypoints The interior waypoints.
+ * @param end The ending pose.
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ public static Trajectory generateTrajectory(
+ Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
+ TrajectoryConfig config
+ ) {
+ var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
+ start, interiorWaypoints.toArray(new Translation2d[0]), end
+ );
+
+ // Return the generated trajectory.
+ return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
+ }
+
+ /**
+ * Generates a trajectory from the given quintic control vectors and config. This method
+ * uses quintic hermite splines -- therefore, all points must be represented by control
+ * vectors. Continuous curvature is guaranteed in this method.
+ *
+ * @param controlVectors List of quintic control vectors.
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ public static Trajectory generateTrajectory(
+ ControlVectorList controlVectors,
+ TrajectoryConfig config
+ ) {
+ final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+ final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
+
+ // Create a new control vector list, flipping the orientation if reversed.
+ for (final var vector : controlVectors) {
+ var newVector = new Spline.ControlVector(vector.x, vector.y);
+ if (config.isReversed()) {
+ newVector.x[1] *= -1;
+ newVector.y[1] *= -1;
+ }
+ newControlVectors.add(newVector);
+ }
+
+ // Get the spline points
+ List<PoseWithCurvature> points;
+ try {
+ points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
+ newControlVectors.toArray(new Spline.ControlVector[]{})
+ ));
+ } catch (MalformedSplineException ex) {
+ DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+ return kDoNothingTrajectory;
+ }
+
+ // Change the points back to their original orientation.
+ if (config.isReversed()) {
+ for (var point : points) {
+ point.poseMeters = point.poseMeters.plus(flip);
+ point.curvatureRadPerMeter *= -1;
+ }
+ }
+
+ // Generate and return trajectory.
+ return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+ config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+ config.getMaxAcceleration(), config.isReversed());
+
+ }
+
+ /**
+ * Generates a trajectory from the given waypoints and config. This method
+ * uses quintic hermite splines -- therefore, all points must be represented by Pose2d
+ * objects. Continuous curvature is guaranteed in this method.
+ *
+ * @param waypoints List of waypoints..
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ @SuppressWarnings("LocalVariableName")
+ public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
+ var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
+ var newList = new ControlVectorList(originalList);
+ return generateTrajectory(newList, config);
+ }
+
+ /**
+ * Generate spline points from a vector of splines by parameterizing the
+ * splines.
+ *
+ * @param splines The splines to parameterize.
+ * @return The spline points for use in time parameterization of a trajectory.
+ * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+ * with approximately opposing headings)
+ */
+ public static List<PoseWithCurvature> splinePointsFromSplines(
+ Spline[] splines) {
+ // Create the vector of spline points.
+ var splinePoints = new ArrayList<PoseWithCurvature>();
+
+ // Add the first point to the vector.
+ splinePoints.add(splines[0].getPoint(0.0));
+
+ // Iterate through the vector and parameterize each spline, adding the
+ // parameterized points to the final vector.
+ for (final var spline : splines) {
+ var points = SplineParameterizer.parameterize(spline);
+
+ // Append the array of poses to the vector. We are removing the first
+ // point because it's a duplicate of the last point from the previous
+ // spline.
+ splinePoints.addAll(points.subList(1, points.size()));
+ }
+ return splinePoints;
+ }
+
+ // Work around type erasure signatures
+ public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
+ public ControlVectorList(int initialCapacity) {
+ super(initialCapacity);
+ }
+
+ public ControlVectorList() {
+ super();
+ }
+
+ public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
+ super(collection);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
new file mode 100644
index 0000000..9459dd0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
@@ -0,0 +1,303 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+/**
+ * Class used to parameterize a trajectory by time.
+ */
+public final class TrajectoryParameterizer {
+ /**
+ * Private constructor because this is a utility class.
+ */
+ private TrajectoryParameterizer() {
+ }
+
+ /**
+ * Parameterize the trajectory by time. This is where the velocity profile is
+ * generated.
+ *
+ * <p>The derivation of the algorithm used can be found
+ * <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
+ * here</a>.
+ *
+ * @param points Reference to the spline points.
+ * @param constraints A vector of various velocity and acceleration.
+ * constraints.
+ * @param startVelocityMetersPerSecond The start velocity for the trajectory.
+ * @param endVelocityMetersPerSecond The end velocity for the trajectory.
+ * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
+ * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+ * @param reversed Whether the robot should move backwards.
+ * Note that the robot will still move from
+ * a -> b -> ... -> z as defined in the
+ * waypoints.
+ * @return The trajectory.
+ */
+ @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
+ "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
+ "PMD.AvoidThrowingRawExceptionTypes"})
+ public static Trajectory timeParameterizeTrajectory(
+ List<PoseWithCurvature> points,
+ List<TrajectoryConstraint> constraints,
+ double startVelocityMetersPerSecond,
+ double endVelocityMetersPerSecond,
+ double maxVelocityMetersPerSecond,
+ double maxAccelerationMetersPerSecondSq,
+ boolean reversed
+ ) {
+ var constrainedStates = new ArrayList<ConstrainedState>(points.size());
+ var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
+ -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
+
+ // Forward pass
+ for (int i = 0; i < points.size(); i++) {
+ constrainedStates.add(new ConstrainedState());
+ var constrainedState = constrainedStates.get(i);
+ constrainedState.pose = points.get(i);
+
+ // Begin constraining based on predecessor.
+ double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
+ predecessor.pose.poseMeters.getTranslation());
+ constrainedState.distanceMeters = predecessor.distanceMeters + ds;
+
+ // We may need to iterate to find the maximum end velocity and common
+ // acceleration, since acceleration limits may be a function of velocity.
+ while (true) {
+ // Enforce global max velocity and max reachable velocity by global
+ // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+ constrainedState.maxVelocityMetersPerSecond = Math.min(
+ maxVelocityMetersPerSecond,
+ Math.sqrt(predecessor.maxVelocityMetersPerSecond
+ * predecessor.maxVelocityMetersPerSecond
+ + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
+ );
+
+ constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
+ constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+
+ // At this point, the constrained state is fully constructed apart from
+ // all the custom-defined user constraints.
+ for (final var constraint : constraints) {
+ constrainedState.maxVelocityMetersPerSecond = Math.min(
+ constrainedState.maxVelocityMetersPerSecond,
+ constraint.getMaxVelocityMetersPerSecond(
+ constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
+ constrainedState.maxVelocityMetersPerSecond)
+ );
+ }
+
+ // Now enforce all acceleration limits.
+ enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+ if (ds < 1E-6) {
+ break;
+ }
+
+ // If the actual acceleration for this state is higher than the max
+ // acceleration that we applied, then we need to reduce the max
+ // acceleration of the predecessor and try again.
+ double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
+ * constrainedState.maxVelocityMetersPerSecond
+ - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
+ / (ds * 2.0);
+
+ // If we violate the max acceleration constraint, let's modify the
+ // predecessor.
+ if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
+ predecessor.maxAccelerationMetersPerSecondSq
+ = constrainedState.maxAccelerationMetersPerSecondSq;
+ } else {
+ // Constrain the predecessor's max acceleration to the current
+ // acceleration.
+ if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
+ predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
+ }
+ // If the actual acceleration is less than the predecessor's min
+ // acceleration, it will be repaired in the backward pass.
+ break;
+ }
+ }
+ predecessor = constrainedState;
+ }
+
+ var successor = new ConstrainedState(points.get(points.size() - 1),
+ constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
+ endVelocityMetersPerSecond,
+ -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
+
+ // Backward pass
+ for (int i = points.size() - 1; i >= 0; i--) {
+ var constrainedState = constrainedStates.get(i);
+ double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
+
+ while (true) {
+ // Enforce max velocity limit (reverse)
+ // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+ double newMaxVelocity = Math.sqrt(
+ successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+ + successor.minAccelerationMetersPerSecondSq * ds * 2.0
+ );
+
+ // No more limits to impose! This state can be finalized.
+ if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
+ break;
+ }
+
+ constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
+
+ // Check all acceleration constraints with the new max velocity.
+ enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+ if (ds > -1E-6) {
+ break;
+ }
+
+ // If the actual acceleration for this state is lower than the min
+ // acceleration, then we need to lower the min acceleration of the
+ // successor and try again.
+ double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
+ * constrainedState.maxVelocityMetersPerSecond
+ - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
+ / (ds * 2.0);
+
+ if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
+ successor.minAccelerationMetersPerSecondSq
+ = constrainedState.minAccelerationMetersPerSecondSq;
+ } else {
+ successor.minAccelerationMetersPerSecondSq = actualAcceleration;
+ break;
+ }
+ }
+ successor = constrainedState;
+ }
+
+ // Now we can integrate the constrained states forward in time to obtain our
+ // trajectory states.
+ var states = new ArrayList<Trajectory.State>(points.size());
+ double timeSeconds = 0.0;
+ double distanceMeters = 0.0;
+ double velocityMetersPerSecond = 0.0;
+
+ for (int i = 0; i < constrainedStates.size(); i++) {
+ final var state = constrainedStates.get(i);
+
+ // Calculate the change in position between the current state and the previous
+ // state.
+ double ds = state.distanceMeters - distanceMeters;
+
+ // Calculate the acceleration between the current state and the previous
+ // state.
+ double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
+ - velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
+
+ // Calculate dt
+ double dt = 0.0;
+ if (i > 0) {
+ states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
+ if (Math.abs(accel) > 1E-6) {
+ // v_f = v_0 + a * t
+ dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
+ } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
+ // delta_x = v * t
+ dt = ds / velocityMetersPerSecond;
+ } else {
+ throw new RuntimeException("Something went wrong");
+ }
+ }
+
+ velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
+ distanceMeters = state.distanceMeters;
+
+ timeSeconds += dt;
+
+ states.add(new Trajectory.State(
+ timeSeconds,
+ reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
+ reversed ? -accel : accel,
+ state.pose.poseMeters, state.pose.curvatureRadPerMeter
+ ));
+ }
+
+ return new Trajectory(states);
+ }
+
+ private static void enforceAccelerationLimits(boolean reverse,
+ List<TrajectoryConstraint> constraints,
+ ConstrainedState state) {
+
+ for (final var constraint : constraints) {
+ double factor = reverse ? -1.0 : 1.0;
+ final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
+ state.pose.poseMeters, state.pose.curvatureRadPerMeter,
+ state.maxVelocityMetersPerSecond * factor);
+
+ state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
+ reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
+ : minMaxAccel.minAccelerationMetersPerSecondSq);
+
+ state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
+ reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
+ : minMaxAccel.maxAccelerationMetersPerSecondSq);
+ }
+
+ }
+
+ @SuppressWarnings("MemberName")
+ private static class ConstrainedState {
+ PoseWithCurvature pose;
+ double distanceMeters;
+ double maxVelocityMetersPerSecond;
+ double minAccelerationMetersPerSecondSq;
+ double maxAccelerationMetersPerSecondSq;
+
+ ConstrainedState(PoseWithCurvature pose, double distanceMeters,
+ double maxVelocityMetersPerSecond,
+ double minAccelerationMetersPerSecondSq,
+ double maxAccelerationMetersPerSecondSq) {
+ this.pose = pose;
+ this.distanceMeters = distanceMeters;
+ this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
+ this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+ this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+ }
+
+ ConstrainedState() {
+ pose = new PoseWithCurvature();
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
new file mode 100644
index 0000000..0cd0f49
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.io.BufferedReader;
+import java.io.BufferedWriter;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Arrays;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.ObjectReader;
+import com.fasterxml.jackson.databind.ObjectWriter;
+
+public final class TrajectoryUtil {
+ private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class);
+ private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class);
+
+ private TrajectoryUtil() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Imports a Trajectory from a PathWeaver-style JSON file.
+ * @param path the path of the json file to import from
+ * @return The trajectory represented by the file.
+ * @throws IOException if reading from the file fails
+ */
+ public static Trajectory fromPathweaverJson(Path path) throws IOException {
+ try (BufferedReader reader = Files.newBufferedReader(path)) {
+ Trajectory.State[] state = READER.readValue(reader);
+ return new Trajectory(Arrays.asList(state));
+ }
+ }
+
+ /**
+ * Exports a Trajectory to a PathWeaver-style JSON file.
+ * @param trajectory the trajectory to export
+ * @param path the path of the file to export to
+ * @throws IOException if writing to the file fails
+ */
+ public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
+ Files.createDirectories(path.getParent());
+ try (BufferedWriter writer = Files.newBufferedWriter(path)) {
+ WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
+ }
+ }
+
+ /**
+ * Deserializes a Trajectory from PathWeaver-style JSON.
+ * @param json the string containing the serialized JSON
+ * @return the trajectory represented by the JSON
+ * @throws JsonProcessingException if deserializing the JSON fails
+ */
+ public static Trajectory deserializeTrajectory(String json) throws JsonProcessingException {
+ Trajectory.State[] state = READER.readValue(json);
+ return new Trajectory(Arrays.asList(state));
+ }
+
+ /**
+ * Serializes a Trajectory to PathWeaver-style JSON.
+ * @param trajectory the trajectory to export
+ * @return the string containing the serialized JSON
+ * @throws JsonProcessingException if serializing the Trajectory fails
+ */
+ public static String serializeTrajectory(Trajectory trajectory) throws JsonProcessingException {
+ return WRITER.writeValueAsString(trajectory.getStates().toArray(new Trajectory.State[0]));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
new file mode 100644
index 0000000..0b24411
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
@@ -0,0 +1,304 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Objects;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * <p>Initialization:
+ * <pre><code>
+ * TrapezoidProfile.Constraints constraints =
+ * new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+ * TrapezoidProfile.State previousProfiledReference =
+ * new TrapezoidProfile.State(initialReference, 0.0);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ * <pre><code>
+ * TrapezoidProfile profile =
+ * new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
+ * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for
+ * `calculate()` and to determine when the profile has completed via
+ * `isFinished()`.
+ */
+public class TrapezoidProfile {
+ // The direction of the profile, either 1 for forwards or -1 for inverted
+ private int m_direction;
+
+ private Constraints m_constraints;
+ private State m_initial;
+ private State m_goal;
+
+ private double m_endAccel;
+ private double m_endFullSpeed;
+ private double m_endDeccel;
+
+ public static class Constraints {
+ @SuppressWarnings("MemberName")
+ public double maxVelocity;
+ @SuppressWarnings("MemberName")
+ public double maxAcceleration;
+
+ public Constraints() {
+ HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
+ }
+
+ /**
+ * Construct constraints for a TrapezoidProfile.
+ *
+ * @param maxVelocity maximum velocity
+ * @param maxAcceleration maximum acceleration
+ */
+ public Constraints(double maxVelocity, double maxAcceleration) {
+ this.maxVelocity = maxVelocity;
+ this.maxAcceleration = maxAcceleration;
+ HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
+ }
+ }
+
+ public static class State {
+ @SuppressWarnings("MemberName")
+ public double position;
+ @SuppressWarnings("MemberName")
+ public double velocity;
+
+ public State() {
+ }
+
+ public State(double position, double velocity) {
+ this.position = position;
+ this.velocity = velocity;
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (other instanceof State) {
+ State rhs = (State) other;
+ return this.position == rhs.position && this.velocity == rhs.velocity;
+ } else {
+ return false;
+ }
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(position, velocity);
+ }
+ }
+
+ /**
+ * Construct a TrapezoidProfile.
+ *
+ * @param constraints The constraints on the profile, like maximum velocity.
+ * @param goal The desired state when the profile is complete.
+ * @param initial The initial state (usually the current state).
+ */
+ public TrapezoidProfile(Constraints constraints, State goal, State initial) {
+ m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
+ m_constraints = constraints;
+ m_initial = direct(initial);
+ m_goal = direct(goal);
+
+ if (m_initial.velocity > m_constraints.maxVelocity) {
+ m_initial.velocity = m_constraints.maxVelocity;
+ }
+
+ // Deal with a possibly truncated motion profile (with nonzero initial or
+ // final velocity) by calculating the parameters as if the profile began and
+ // ended at zero velocity
+ double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+ double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+ double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+ double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+ // Now we can calculate the parameters as if it was a full trapezoid instead
+ // of a truncated one
+
+ double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
+ + cutoffDistEnd;
+ double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+ double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
+ * m_constraints.maxAcceleration;
+
+ // Handle the case where the profile never reaches full speed
+ if (fullSpeedDist < 0) {
+ accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+ fullSpeedDist = 0;
+ }
+
+ m_endAccel = accelerationTime - cutoffBegin;
+ m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+ m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+ }
+
+ /**
+ * Construct a TrapezoidProfile.
+ *
+ * @param constraints The constraints on the profile, like maximum velocity.
+ * @param goal The desired state when the profile is complete.
+ */
+ public TrapezoidProfile(Constraints constraints, State goal) {
+ this(constraints, goal, new State(0, 0));
+ }
+
+ /**
+ * Calculate the correct position and velocity for the profile at a time t
+ * where the beginning of the profile was at time t = 0.
+ *
+ * @param t The time since the beginning of the profile.
+ */
+ @SuppressWarnings("ParameterName")
+ public State calculate(double t) {
+ State result = m_initial;
+
+ if (t < m_endAccel) {
+ result.velocity += t * m_constraints.maxAcceleration;
+ result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+ } else if (t < m_endFullSpeed) {
+ result.velocity = m_constraints.maxVelocity;
+ result.position += (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration
+ / 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel);
+ } else if (t <= m_endDeccel) {
+ result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+ double timeLeft = m_endDeccel - t;
+ result.position = m_goal.position - (m_goal.velocity + timeLeft
+ * m_constraints.maxAcceleration / 2.0) * timeLeft;
+ } else {
+ result = m_goal;
+ }
+
+ return direct(result);
+ }
+
+ /**
+ * Returns the time left until a target distance in the profile is reached.
+ *
+ * @param target The target distance.
+ */
+ public double timeLeftUntil(double target) {
+ double position = m_initial.position * m_direction;
+ double velocity = m_initial.velocity * m_direction;
+
+ double endAccel = m_endAccel * m_direction;
+ double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+ if (target < position) {
+ endAccel = -endAccel;
+ endFullSpeed = -endFullSpeed;
+ velocity = -velocity;
+ }
+
+ endAccel = Math.max(endAccel, 0);
+ endFullSpeed = Math.max(endFullSpeed, 0);
+ double endDeccel = m_endDeccel - endAccel - endFullSpeed;
+ endDeccel = Math.max(endDeccel, 0);
+
+ final double acceleration = m_constraints.maxAcceleration;
+ final double decceleration = -m_constraints.maxAcceleration;
+
+ double distToTarget = Math.abs(target - position);
+ if (distToTarget < 1e-6) {
+ return 0;
+ }
+
+ double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+ double deccelVelocity;
+ if (endAccel > 0) {
+ deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
+ } else {
+ deccelVelocity = velocity;
+ }
+
+ double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+ deccelDist = Math.max(deccelDist, 0);
+
+ double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+ if (accelDist > distToTarget) {
+ accelDist = distToTarget;
+ fullSpeedDist = 0;
+ deccelDist = 0;
+ } else if (accelDist + fullSpeedDist > distToTarget) {
+ fullSpeedDist = distToTarget - accelDist;
+ deccelDist = 0;
+ } else {
+ deccelDist = distToTarget - fullSpeedDist - accelDist;
+ }
+
+ double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
+ * accelDist))) / acceleration;
+
+ double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
+ + 2 * decceleration * deccelDist))) / decceleration;
+
+ double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+ return accelTime + fullSpeedTime + deccelTime;
+ }
+
+ /**
+ * Returns the total time the profile takes to reach the goal.
+ */
+ public double totalTime() {
+ return m_endDeccel;
+ }
+
+ /**
+ * Returns true if the profile has reached the goal.
+ *
+ * <p>The profile has reached the goal if the time since the profile started
+ * has exceeded the profile's total time.
+ *
+ * @param t The time since the beginning of the profile.
+ */
+ @SuppressWarnings("ParameterName")
+ public boolean isFinished(double t) {
+ return t >= totalTime();
+ }
+
+ /**
+ * Returns true if the profile inverted.
+ *
+ * <p>The profile is inverted if goal position is less than the initial position.
+ *
+ * @param initial The initial state (usually the current state).
+ * @param goal The desired state when the profile is complete.
+ */
+ private static boolean shouldFlipAcceleration(State initial, State goal) {
+ return initial.position > goal.position;
+ }
+
+ // Flip the sign of the velocity and position if the profile is inverted
+ private State direct(State in) {
+ State result = new State(in.position, in.velocity);
+ result.position = result.position * m_direction;
+ result.velocity = result.velocity * m_direction;
+ return result;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
new file mode 100644
index 0000000..f74e7b4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when
+ * traversing a trajectory. The centripetal acceleration of a robot is defined
+ * as the velocity squared divided by the radius of curvature.
+ *
+ * <p>Effectively, limiting the maximum centripetal acceleration will cause the
+ * robot to slow down around tight turns, making it easier to track trajectories
+ * with sharp turns.
+ */
+public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
+ private final double m_maxCentripetalAccelerationMetersPerSecondSq;
+
+ /**
+ * Constructs a centripetal acceleration constraint.
+ *
+ * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
+ */
+ public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
+ m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
+ }
+
+ /**
+ * Returns the max velocity given the current pose and curvature.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+ * constraints are applied.
+ * @return The absolute maximum velocity.
+ */
+ @Override
+ public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ // ac = v^2 / r
+ // k (curvature) = 1 / r
+
+ // therefore, ac = v^2 * k
+ // ac / k = v^2
+ // v = std::sqrt(ac / k)
+
+ return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
+ / Math.abs(curvatureRadPerMeter));
+ }
+
+ /**
+ * Returns the minimum and maximum allowable acceleration for the trajectory
+ * given pose, curvature, and speed.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+ * @return The min and max acceleration bounds.
+ */
+ @Override
+ public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+ double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ // The acceleration of the robot has no impact on the centripetal acceleration
+ // of the robot.
+ return new MinMax();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
new file mode 100644
index 0000000..9a57720
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
+ private final double m_maxSpeedMetersPerSecond;
+ private final DifferentialDriveKinematics m_kinematics;
+
+ /**
+ * Constructs a differential drive dynamics constraint.
+ *
+ * @param kinematics A kinematics component describing the drive geometry.
+ * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+ */
+ public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
+ double maxSpeedMetersPerSecond) {
+ m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+ m_kinematics = kinematics;
+ }
+
+
+ /**
+ * Returns the max velocity given the current pose and curvature.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+ * constraints are applied.
+ * @return The absolute maximum velocity.
+ */
+ @Override
+ public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ // Create an object to represent the current chassis speeds.
+ var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
+ 0, velocityMetersPerSecond * curvatureRadPerMeter);
+
+ // Get the wheel speeds and normalize them to within the max velocity.
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+ wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+ // Return the new linear chassis speed.
+ return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
+ }
+
+ /**
+ * Returns the minimum and maximum allowable acceleration for the trajectory
+ * given pose, curvature, and speed.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+ * @return The min and max acceleration bounds.
+ */
+ @Override
+ public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+ double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ return new MinMax();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
new file mode 100644
index 0000000..71432bf
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A class that enforces constraints on differential drive voltage expenditure based on the motor
+ * dynamics and the drive kinematics. Ensures that the acceleration of any wheel of the robot
+ * while following the trajectory is never higher than what can be achieved with the given
+ * maximum voltage.
+ */
+public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
+ private final SimpleMotorFeedforward m_feedforward;
+ private final DifferentialDriveKinematics m_kinematics;
+ private final double m_maxVoltage;
+
+ /**
+ * Creates a new DifferentialDriveVoltageConstraint.
+ *
+ * @param feedforward A feedforward component describing the behavior of the drive.
+ * @param kinematics A kinematics component describing the drive geometry.
+ * @param maxVoltage The maximum voltage available to the motors while following the path.
+ * Should be somewhat less than the nominal battery voltage (12V) to account
+ * for "voltage sag" due to current draw.
+ */
+ public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward,
+ DifferentialDriveKinematics kinematics,
+ double maxVoltage) {
+ m_feedforward = requireNonNullParam(feedforward, "feedforward",
+ "DifferentialDriveVoltageConstraint");
+ m_kinematics = requireNonNullParam(kinematics, "kinematics",
+ "DifferentialDriveVoltageConstraint");
+ m_maxVoltage = maxVoltage;
+ }
+
+ @Override
+ public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ return Double.POSITIVE_INFINITY;
+ }
+
+ @Override
+ public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+ double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0,
+ velocityMetersPerSecond
+ * curvatureRadPerMeter));
+
+ double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond,
+ wheelSpeeds.rightMetersPerSecond);
+ double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond,
+ wheelSpeeds.rightMetersPerSecond);
+
+ // Calculate maximum/minimum possible accelerations from motor dynamics
+ // and max/min wheel speeds
+ double maxWheelAcceleration =
+ m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+ double minWheelAcceleration =
+ m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+ // Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
+ // increased by half of the trackwidth T. Inner wheel has radius decreased
+ // by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so
+ // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
+ // Inner wheel is similar.
+
+ // sgn(speed) term added to correctly account for which wheel is on
+ // outside of turn:
+ // If moving forward, max acceleration constraint corresponds to wheel on outside of turn
+ // If moving backward, max acceleration constraint corresponds to wheel on inside of turn
+
+ double maxChassisAcceleration =
+ maxWheelAcceleration
+ / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
+ * Math.signum(velocityMetersPerSecond) / 2);
+ double minChassisAcceleration =
+ minWheelAcceleration
+ / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
+ * Math.signum(velocityMetersPerSecond) / 2);
+
+ // Negate acceleration of wheel on inside of turn if center of turn is inside of wheelbase
+ if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
+ if (velocityMetersPerSecond > 0) {
+ minChassisAcceleration = -minChassisAcceleration;
+ } else {
+ maxChassisAcceleration = -maxChassisAcceleration;
+ }
+ }
+
+ return new MinMax(minChassisAcceleration, maxChassisAcceleration);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
new file mode 100644
index 0000000..dfa2583
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+
+/**
+ * A class that enforces constraints on the mecanum drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for all 4 wheels of the drivetrain stay below a certain
+ * limit.
+ */
+public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
+ private final double m_maxSpeedMetersPerSecond;
+ private final MecanumDriveKinematics m_kinematics;
+
+ /**
+ * Constructs a mecanum drive dynamics constraint.
+ *
+ * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+ */
+ public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
+ double maxSpeedMetersPerSecond) {
+ m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+ m_kinematics = kinematics;
+ }
+
+
+ /**
+ * Returns the max velocity given the current pose and curvature.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+ * constraints are applied.
+ * @return The absolute maximum velocity.
+ */
+ @Override
+ public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ // Represents the velocity of the chassis in the x direction
+ var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+ // Represents the velocity of the chassis in the y direction
+ var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+ // Create an object to represent the current chassis speeds.
+ var chassisSpeeds = new ChassisSpeeds(xdVelocity,
+ ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+ // Get the wheel speeds and normalize them to within the max velocity.
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+ wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+ // Convert normalized wheel speeds back to chassis speeds
+ var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ // Return the new linear chassis speed.
+ return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+ }
+
+ /**
+ * Returns the minimum and maximum allowable acceleration for the trajectory
+ * given pose, curvature, and speed.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+ * @return The min and max acceleration bounds.
+ */
+ @Override
+ public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+ double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ return new MinMax();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
new file mode 100644
index 0000000..8ca63f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for all 4 wheels of the drivetrain stay below a certain
+ * limit.
+ */
+public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
+ private final double m_maxSpeedMetersPerSecond;
+ private final SwerveDriveKinematics m_kinematics;
+
+ /**
+ * Constructs a swerve drive dynamics constraint.
+ *
+ * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+ */
+ public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
+ double maxSpeedMetersPerSecond) {
+ m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+ m_kinematics = kinematics;
+ }
+
+
+ /**
+ * Returns the max velocity given the current pose and curvature.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+ * constraints are applied.
+ * @return The absolute maximum velocity.
+ */
+ @Override
+ public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ // Represents the velocity of the chassis in the x direction
+ var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+ // Represents the velocity of the chassis in the y direction
+ var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+ // Create an object to represent the current chassis speeds.
+ var chassisSpeeds = new ChassisSpeeds(xdVelocity,
+ ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+ // Get the wheel speeds and normalize them to within the max velocity.
+ var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
+ SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
+
+ // Convert normalized wheel speeds back to chassis speeds
+ var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ // Return the new linear chassis speed.
+ return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+ }
+
+ /**
+ * Returns the minimum and maximum allowable acceleration for the trajectory
+ * given pose, curvature, and speed.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+ * @return The min and max acceleration bounds.
+ */
+ @Override
+ public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+ double curvatureRadPerMeter,
+ double velocityMetersPerSecond) {
+ return new MinMax();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
new file mode 100644
index 0000000..b338c3f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * An interface for defining user-defined velocity and acceleration constraints
+ * while generating trajectories.
+ */
+public interface TrajectoryConstraint {
+
+ /**
+ * Returns the max velocity given the current pose and curvature.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+ * constraints are applied.
+ * @return The absolute maximum velocity.
+ */
+ double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond);
+
+ /**
+ * Returns the minimum and maximum allowable acceleration for the trajectory
+ * given pose, curvature, and speed.
+ *
+ * @param poseMeters The pose at the current point in the trajectory.
+ * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+ * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+ * @return The min and max acceleration bounds.
+ */
+ MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
+ double velocityMetersPerSecond);
+
+ /**
+ * Represents a minimum and maximum acceleration.
+ */
+ @SuppressWarnings("MemberName")
+ class MinMax {
+ public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
+ public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
+
+ /**
+ * Constructs a MinMax.
+ *
+ * @param minAccelerationMetersPerSecondSq The minimum acceleration.
+ * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
+ */
+ public MinMax(double minAccelerationMetersPerSecondSq,
+ double maxAccelerationMetersPerSecondSq) {
+ this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+ this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+ }
+
+ /**
+ * Constructs a MinMax with default values.
+ */
+ public MinMax() {
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
new file mode 100644
index 0000000..e4d5b69
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -0,0 +1,798 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.Objects;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Represents colors.
+ *
+ * <p>Limited to 12 bits of precision.
+ */
+@SuppressWarnings("MemberName")
+public class Color {
+ /*
+ * FIRST Colors
+ */
+
+ /**
+ * #1560BD.
+ */
+ public static final Color kDenim = new Color(0.0823529412, 0.376470589, 0.7411764706);
+
+ /**
+ * #0066B3.
+ */
+ public static final Color kFirstBlue = new Color(0.0, 0.4, 0.7019607844);
+
+ /**
+ * #ED1C24.
+ */
+ public static final Color kFirstRed = new Color(0.9294117648, 0.1098039216, 0.1411764706);
+
+ /*
+ * Standard Colors
+ */
+
+ /**
+ * #F0F8FF.
+ */
+ public static final Color kAliceBlue = new Color(0.9411765f, 0.972549f, 1.0f);
+
+ /**
+ * #FAEBD7.
+ */
+ public static final Color kAntiqueWhite = new Color(0.98039216f, 0.92156863f, 0.84313726f);
+
+ /**
+ * #00FFFF.
+ */
+ public static final Color kAqua = new Color(0.0f, 1.0f, 1.0f);
+
+ /**
+ * #7FFFD4.
+ */
+ public static final Color kAquamarine = new Color(0.49803922f, 1.0f, 0.83137256f);
+
+ /**
+ * #F0FFFF.
+ */
+ public static final Color kAzure = new Color(0.9411765f, 1.0f, 1.0f);
+
+ /**
+ * #F5F5DC.
+ */
+ public static final Color kBeige = new Color(0.9607843f, 0.9607843f, 0.8627451f);
+
+ /**
+ * #FFE4C4.
+ */
+ public static final Color kBisque = new Color(1.0f, 0.89411765f, 0.76862746f);
+
+ /**
+ * #000000.
+ */
+ public static final Color kBlack = new Color(0.0f, 0.0f, 0.0f);
+
+ /**
+ * #FFEBCD.
+ */
+ public static final Color kBlanchedAlmond = new Color(1.0f, 0.92156863f, 0.8039216f);
+
+ /**
+ * #0000FF.
+ */
+ public static final Color kBlue = new Color(0.0f, 0.0f, 1.0f);
+
+ /**
+ * #8A2BE2.
+ */
+ public static final Color kBlueViolet = new Color(0.5411765f, 0.16862746f, 0.8862745f);
+
+ /**
+ * #A52A2A.
+ */
+ public static final Color kBrown = new Color(0.64705884f, 0.16470589f, 0.16470589f);
+
+ /**
+ * #DEB887.
+ */
+ public static final Color kBurlywood = new Color(0.87058824f, 0.72156864f, 0.5294118f);
+
+ /**
+ * #5F9EA0.
+ */
+ public static final Color kCadetBlue = new Color(0.37254903f, 0.61960787f, 0.627451f);
+
+ /**
+ * #7FFF00.
+ */
+ public static final Color kChartreuse = new Color(0.49803922f, 1.0f, 0.0f);
+
+ /**
+ * #D2691E.
+ */
+ public static final Color kChocolate = new Color(0.8235294f, 0.4117647f, 0.11764706f);
+
+ /**
+ * #FF7F50.
+ */
+ public static final Color kCoral = new Color(1.0f, 0.49803922f, 0.3137255f);
+
+ /**
+ * #6495ED.
+ */
+ public static final Color kCornflowerBlue = new Color(0.39215687f, 0.58431375f, 0.92941177f);
+
+ /**
+ * #FFF8DC.
+ */
+ public static final Color kCornsilk = new Color(1.0f, 0.972549f, 0.8627451f);
+
+ /**
+ * #DC143C.
+ */
+ public static final Color kCrimson = new Color(0.8627451f, 0.078431375f, 0.23529412f);
+
+ /**
+ * #00FFFF.
+ */
+ public static final Color kCyan = new Color(0.0f, 1.0f, 1.0f);
+
+ /**
+ * #00008B.
+ */
+ public static final Color kDarkBlue = new Color(0.0f, 0.0f, 0.54509807f);
+
+ /**
+ * #008B8B.
+ */
+ public static final Color kDarkCyan = new Color(0.0f, 0.54509807f, 0.54509807f);
+
+ /**
+ * #B8860B.
+ */
+ public static final Color kDarkGoldenrod = new Color(0.72156864f, 0.5254902f, 0.043137256f);
+
+ /**
+ * #A9A9A9.
+ */
+ public static final Color kDarkGray = new Color(0.6627451f, 0.6627451f, 0.6627451f);
+
+ /**
+ * #006400.
+ */
+ public static final Color kDarkGreen = new Color(0.0f, 0.39215687f, 0.0f);
+
+ /**
+ * #BDB76B.
+ */
+ public static final Color kDarkKhaki = new Color(0.7411765f, 0.7176471f, 0.41960785f);
+
+ /**
+ * #8B008B.
+ */
+ public static final Color kDarkMagenta = new Color(0.54509807f, 0.0f, 0.54509807f);
+
+ /**
+ * #556B2F.
+ */
+ public static final Color kDarkOliveGreen = new Color(0.33333334f, 0.41960785f, 0.18431373f);
+
+ /**
+ * #FF8C00.
+ */
+ public static final Color kDarkOrange = new Color(1.0f, 0.54901963f, 0.0f);
+
+ /**
+ * #9932CC.
+ */
+ public static final Color kDarkOrchid = new Color(0.6f, 0.19607843f, 0.8f);
+
+ /**
+ * #8B0000.
+ */
+ public static final Color kDarkRed = new Color(0.54509807f, 0.0f, 0.0f);
+
+ /**
+ * #E9967A.
+ */
+ public static final Color kDarkSalmon = new Color(0.9137255f, 0.5882353f, 0.47843137f);
+
+ /**
+ * #8FBC8F.
+ */
+ public static final Color kDarkSeaGreen = new Color(0.56078434f, 0.7372549f, 0.56078434f);
+
+ /**
+ * #483D8B.
+ */
+ public static final Color kDarkSlateBlue = new Color(0.28235295f, 0.23921569f, 0.54509807f);
+
+ /**
+ * #2F4F4F.
+ */
+ public static final Color kDarkSlateGray = new Color(0.18431373f, 0.30980393f, 0.30980393f);
+
+ /**
+ * #00CED1.
+ */
+ public static final Color kDarkTurquoise = new Color(0.0f, 0.80784315f, 0.81960785f);
+
+ /**
+ * #9400D3.
+ */
+ public static final Color kDarkViolet = new Color(0.5803922f, 0.0f, 0.827451f);
+
+ /**
+ * #FF1493.
+ */
+ public static final Color kDeepPink = new Color(1.0f, 0.078431375f, 0.5764706f);
+
+ /**
+ * #00BFFF.
+ */
+ public static final Color kDeepSkyBlue = new Color(0.0f, 0.7490196f, 1.0f);
+
+ /**
+ * #696969.
+ */
+ public static final Color kDimGray = new Color(0.4117647f, 0.4117647f, 0.4117647f);
+
+ /**
+ * #1E90FF.
+ */
+ public static final Color kDodgerBlue = new Color(0.11764706f, 0.5647059f, 1.0f);
+
+ /**
+ * #B22222.
+ */
+ public static final Color kFirebrick = new Color(0.69803923f, 0.13333334f, 0.13333334f);
+
+ /**
+ * #FFFAF0.
+ */
+ public static final Color kFloralWhite = new Color(1.0f, 0.98039216f, 0.9411765f);
+
+ /**
+ * #228B22.
+ */
+ public static final Color kForestGreen = new Color(0.13333334f, 0.54509807f, 0.13333334f);
+
+ /**
+ * #FF00FF.
+ */
+ public static final Color kFuchsia = new Color(1.0f, 0.0f, 1.0f);
+
+ /**
+ * #DCDCDC.
+ */
+ public static final Color kGainsboro = new Color(0.8627451f, 0.8627451f, 0.8627451f);
+
+ /**
+ * #F8F8FF.
+ */
+ public static final Color kGhostWhite = new Color(0.972549f, 0.972549f, 1.0f);
+
+ /**
+ * #FFD700.
+ */
+ public static final Color kGold = new Color(1.0f, 0.84313726f, 0.0f);
+
+ /**
+ * #DAA520.
+ */
+ public static final Color kGoldenrod = new Color(0.85490197f, 0.64705884f, 0.1254902f);
+
+ /**
+ * #808080.
+ */
+ public static final Color kGray = new Color(0.5019608f, 0.5019608f, 0.5019608f);
+
+ /**
+ * #008000.
+ */
+ public static final Color kGreen = new Color(0.0f, 0.5019608f, 0.0f);
+
+ /**
+ * #ADFF2F.
+ */
+ public static final Color kGreenYellow = new Color(0.6784314f, 1.0f, 0.18431373f);
+
+ /**
+ * #F0FFF0.
+ */
+ public static final Color kHoneydew = new Color(0.9411765f, 1.0f, 0.9411765f);
+
+ /**
+ * #FF69B4.
+ */
+ public static final Color kHotPink = new Color(1.0f, 0.4117647f, 0.7058824f);
+
+ /**
+ * #CD5C5C.
+ */
+ public static final Color kIndianRed = new Color(0.8039216f, 0.36078432f, 0.36078432f);
+
+ /**
+ * #4B0082.
+ */
+ public static final Color kIndigo = new Color(0.29411766f, 0.0f, 0.50980395f);
+
+ /**
+ * #FFFFF0.
+ */
+ public static final Color kIvory = new Color(1.0f, 1.0f, 0.9411765f);
+
+ /**
+ * #F0E68C.
+ */
+ public static final Color kKhaki = new Color(0.9411765f, 0.9019608f, 0.54901963f);
+
+ /**
+ * #E6E6FA.
+ */
+ public static final Color kLavender = new Color(0.9019608f, 0.9019608f, 0.98039216f);
+
+ /**
+ * #FFF0F5.
+ */
+ public static final Color kLavenderBlush = new Color(1.0f, 0.9411765f, 0.9607843f);
+
+ /**
+ * #7CFC00.
+ */
+ public static final Color kLawnGreen = new Color(0.4862745f, 0.9882353f, 0.0f);
+
+ /**
+ * #FFFACD.
+ */
+ public static final Color kLemonChiffon = new Color(1.0f, 0.98039216f, 0.8039216f);
+
+ /**
+ * #ADD8E6.
+ */
+ public static final Color kLightBlue = new Color(0.6784314f, 0.84705883f, 0.9019608f);
+
+ /**
+ * #F08080.
+ */
+ public static final Color kLightCoral = new Color(0.9411765f, 0.5019608f, 0.5019608f);
+
+ /**
+ * #E0FFFF.
+ */
+ public static final Color kLightCyan = new Color(0.8784314f, 1.0f, 1.0f);
+
+ /**
+ * #FAFAD2.
+ */
+ public static final Color kLightGoldenrodYellow = new Color(0.98039216f, 0.98039216f, 0.8235294f);
+
+ /**
+ * #D3D3D3.
+ */
+ public static final Color kLightGray = new Color(0.827451f, 0.827451f, 0.827451f);
+
+ /**
+ * #90EE90.
+ */
+ public static final Color kLightGreen = new Color(0.5647059f, 0.93333334f, 0.5647059f);
+
+ /**
+ * #FFB6C1.
+ */
+ public static final Color kLightPink = new Color(1.0f, 0.7137255f, 0.75686276f);
+
+ /**
+ * #FFA07A.
+ */
+ public static final Color kLightSalmon = new Color(1.0f, 0.627451f, 0.47843137f);
+
+ /**
+ * #20B2AA.
+ */
+ public static final Color kLightSeagGeen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
+
+ /**
+ * #87CEFA.
+ */
+ public static final Color kLightSkyBlue = new Color(0.5294118f, 0.80784315f, 0.98039216f);
+
+ /**
+ * #778899.
+ */
+ public static final Color kLightSlateGray = new Color(0.46666667f, 0.53333336f, 0.6f);
+
+ /**
+ * #B0C4DE.
+ */
+ public static final Color kLightSteellue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
+
+ /**
+ * #FFFFE0.
+ */
+ public static final Color kLightYellow = new Color(1.0f, 1.0f, 0.8784314f);
+
+ /**
+ * #00FF00.
+ */
+ public static final Color kLime = new Color(0.0f, 1.0f, 0.0f);
+
+ /**
+ * #32CD32.
+ */
+ public static final Color kLimeGreen = new Color(0.19607843f, 0.8039216f, 0.19607843f);
+
+ /**
+ * #FAF0E6.
+ */
+ public static final Color kLinen = new Color(0.98039216f, 0.9411765f, 0.9019608f);
+
+ /**
+ * #FF00FF.
+ */
+ public static final Color kMagenta = new Color(1.0f, 0.0f, 1.0f);
+
+ /**
+ * #800000.
+ */
+ public static final Color kMaroon = new Color(0.5019608f, 0.0f, 0.0f);
+
+ /**
+ * #66CDAA.
+ */
+ public static final Color kMediumAquamarine = new Color(0.4f, 0.8039216f, 0.6666667f);
+
+ /**
+ * #0000CD.
+ */
+ public static final Color kMediumBlue = new Color(0.0f, 0.0f, 0.8039216f);
+
+ /**
+ * #BA55D3.
+ */
+ public static final Color kMediumOrchid = new Color(0.7294118f, 0.33333334f, 0.827451f);
+
+ /**
+ * #9370DB.
+ */
+ public static final Color kMediumPurple = new Color(0.5764706f, 0.4392157f, 0.85882354f);
+
+ /**
+ * #3CB371.
+ */
+ public static final Color kMediumSeaGreen = new Color(0.23529412f, 0.7019608f, 0.44313726f);
+
+ /**
+ * #7B68EE.
+ */
+ public static final Color kMediumSlateBlue = new Color(0.48235294f, 0.40784314f, 0.93333334f);
+
+ /**
+ * #00FA9A.
+ */
+ public static final Color kMediumSpringGreen = new Color(0.0f, 0.98039216f, 0.6039216f);
+
+ /**
+ * #48D1CC.
+ */
+ public static final Color kMediumTurquoise = new Color(0.28235295f, 0.81960785f, 0.8f);
+
+ /**
+ * #C71585.
+ */
+ public static final Color kMediumVioletRed = new Color(0.78039217f, 0.08235294f, 0.52156866f);
+
+ /**
+ * #191970.
+ */
+ public static final Color kMidnightBlue = new Color(0.09803922f, 0.09803922f, 0.4392157f);
+
+ /**
+ * #F5FFFA.
+ */
+ public static final Color kMintcream = new Color(0.9607843f, 1.0f, 0.98039216f);
+
+ /**
+ * #FFE4E1.
+ */
+ public static final Color kMistyRose = new Color(1.0f, 0.89411765f, 0.88235295f);
+
+ /**
+ * #FFE4B5.
+ */
+ public static final Color kMoccasin = new Color(1.0f, 0.89411765f, 0.70980394f);
+
+ /**
+ * #FFDEAD.
+ */
+ public static final Color kNavajoWhite = new Color(1.0f, 0.87058824f, 0.6784314f);
+
+ /**
+ * #000080.
+ */
+ public static final Color kNavy = new Color(0.0f, 0.0f, 0.5019608f);
+
+ /**
+ * #FDF5E6.
+ */
+ public static final Color kOldLace = new Color(0.99215686f, 0.9607843f, 0.9019608f);
+
+ /**
+ * #808000.
+ */
+ public static final Color kOlive = new Color(0.5019608f, 0.5019608f, 0.0f);
+
+ /**
+ * #6B8E23.
+ */
+ public static final Color kOliveDrab = new Color(0.41960785f, 0.5568628f, 0.13725491f);
+
+ /**
+ * #FFA500.
+ */
+ public static final Color kOrange = new Color(1.0f, 0.64705884f, 0.0f);
+
+ /**
+ * #FF4500.
+ */
+ public static final Color kOrangeRed = new Color(1.0f, 0.27058825f, 0.0f);
+
+ /**
+ * #DA70D6.
+ */
+ public static final Color kOrchid = new Color(0.85490197f, 0.4392157f, 0.8392157f);
+
+ /**
+ * #EEE8AA.
+ */
+ public static final Color kPaleGoldenrod = new Color(0.93333334f, 0.9098039f, 0.6666667f);
+
+ /**
+ * #98FB98.
+ */
+ public static final Color kPaleGreen = new Color(0.59607846f, 0.9843137f, 0.59607846f);
+
+ /**
+ * #AFEEEE.
+ */
+ public static final Color kPaleTurquoise = new Color(0.6862745f, 0.93333334f, 0.93333334f);
+
+ /**
+ * #DB7093.
+ */
+ public static final Color kPaleVioletRed = new Color(0.85882354f, 0.4392157f, 0.5764706f);
+
+ /**
+ * #FFEFD5.
+ */
+ public static final Color kPapayaWhip = new Color(1.0f, 0.9372549f, 0.8352941f);
+
+ /**
+ * #FFDAB9.
+ */
+ public static final Color kPeachPuff = new Color(1.0f, 0.85490197f, 0.7254902f);
+
+ /**
+ * #CD853F.
+ */
+ public static final Color kPeru = new Color(0.8039216f, 0.52156866f, 0.24705882f);
+
+ /**
+ * #FFC0CB.
+ */
+ public static final Color kPink = new Color(1.0f, 0.7529412f, 0.79607844f);
+
+ /**
+ * #DDA0DD.
+ */
+ public static final Color kPlum = new Color(0.8666667f, 0.627451f, 0.8666667f);
+
+ /**
+ * #B0E0E6.
+ */
+ public static final Color kPowderBlue = new Color(0.6901961f, 0.8784314f, 0.9019608f);
+
+ /**
+ * #800080.
+ */
+ public static final Color kPurple = new Color(0.5019608f, 0.0f, 0.5019608f);
+
+ /**
+ * #FF0000.
+ */
+ public static final Color kRed = new Color(1.0f, 0.0f, 0.0f);
+
+ /**
+ * #BC8F8F.
+ */
+ public static final Color kRosyBrown = new Color(0.7372549f, 0.56078434f, 0.56078434f);
+
+ /**
+ * #4169E1.
+ */
+ public static final Color kRoyalBlue = new Color(0.25490198f, 0.4117647f, 0.88235295f);
+
+ /**
+ * #8B4513.
+ */
+ public static final Color kSaddleBrown = new Color(0.54509807f, 0.27058825f, 0.07450981f);
+
+ /**
+ * #FA8072.
+ */
+ public static final Color kSalmon = new Color(0.98039216f, 0.5019608f, 0.44705883f);
+
+ /**
+ * #F4A460.
+ */
+ public static final Color kSandyBrown = new Color(0.95686275f, 0.6431373f, 0.3764706f);
+
+ /**
+ * #2E8B57.
+ */
+ public static final Color kSeaGreen = new Color(0.18039216f, 0.54509807f, 0.34117648f);
+
+ /**
+ * #FFF5EE.
+ */
+ public static final Color kSeashell = new Color(1.0f, 0.9607843f, 0.93333334f);
+
+ /**
+ * #A0522D.
+ */
+ public static final Color kSienna = new Color(0.627451f, 0.32156864f, 0.1764706f);
+
+ /**
+ * #C0C0C0.
+ */
+ public static final Color kSilver = new Color(0.7529412f, 0.7529412f, 0.7529412f);
+
+ /**
+ * #87CEEB.
+ */
+ public static final Color kSkyBlue = new Color(0.5294118f, 0.80784315f, 0.92156863f);
+
+ /**
+ * #6A5ACD.
+ */
+ public static final Color kSlateBlue = new Color(0.41568628f, 0.3529412f, 0.8039216f);
+
+ /**
+ * #708090.
+ */
+ public static final Color kSlateGray = new Color(0.4392157f, 0.5019608f, 0.5647059f);
+
+ /**
+ * #FFFAFA.
+ */
+ public static final Color kSnow = new Color(1.0f, 0.98039216f, 0.98039216f);
+
+ /**
+ * #00FF7F.
+ */
+ public static final Color kSpringGreen = new Color(0.0f, 1.0f, 0.49803922f);
+
+ /**
+ * #4682B4.
+ */
+ public static final Color kSteelBlue = new Color(0.27450982f, 0.50980395f, 0.7058824f);
+
+ /**
+ * #D2B48C.
+ */
+ public static final Color kTan = new Color(0.8235294f, 0.7058824f, 0.54901963f);
+
+ /**
+ * #008080.
+ */
+ public static final Color kTeal = new Color(0.0f, 0.5019608f, 0.5019608f);
+
+ /**
+ * #D8BFD8.
+ */
+ public static final Color kThistle = new Color(0.84705883f, 0.7490196f, 0.84705883f);
+
+ /**
+ * #FF6347.
+ */
+ public static final Color kTomato = new Color(1.0f, 0.3882353f, 0.2784314f);
+
+ /**
+ * #40E0D0.
+ */
+ public static final Color kTurquoise = new Color(0.2509804f, 0.8784314f, 0.8156863f);
+
+ /**
+ * #EE82EE.
+ */
+ public static final Color kViolet = new Color(0.93333334f, 0.50980395f, 0.93333334f);
+
+ /**
+ * #F5DEB3.
+ */
+ public static final Color kWheat = new Color(0.9607843f, 0.87058824f, 0.7019608f);
+
+ /**
+ * #FFFFFF.
+ */
+ public static final Color kWhite = new Color(1.0f, 1.0f, 1.0f);
+
+ /**
+ * #F5F5F5.
+ */
+ public static final Color kWhiteSmoke = new Color(0.9607843f, 0.9607843f, 0.9607843f);
+
+ /**
+ * #FFFF00.
+ */
+ public static final Color kYellow = new Color(1.0f, 1.0f, 0.0f);
+
+ /**
+ * #9ACD32.
+ */
+ public static final Color kYellowGreen = new Color(0.6039216f, 0.8039216f, 0.19607843f);
+
+ public final double red;
+ public final double green;
+ public final double blue;
+
+ private static final double kPrecision = Math.pow(2, -12);
+
+ /**
+ * Constructs a Color.
+ *
+ * @param red Red value (0-1)
+ * @param green Green value (0-1)
+ * @param blue Blue value (0-1)
+ */
+ Color(double red, double green, double blue) {
+ this.red = roundAndClamp(red);
+ this.green = roundAndClamp(green);
+ this.blue = roundAndClamp(blue);
+ }
+
+ /**
+ * Constructs a Color from a Color8Bit.
+ *
+ * @param color The color
+ */
+ public Color(Color8Bit color) {
+ this(color.red / 255.0,
+ color.green / 255.0,
+ color.blue / 255.0);
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (this == other) {
+ return true;
+ }
+ if (other == null || getClass() != other.getClass()) {
+ return false;
+ }
+
+ Color color = (Color) other;
+ return Double.compare(color.red, red) == 0
+ && Double.compare(color.green, green) == 0
+ && Double.compare(color.blue, blue) == 0;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(red, green, blue);
+ }
+
+ private static double roundAndClamp(double value) {
+ final var rounded = Math.round(value / kPrecision) * kPrecision;
+ return MathUtil.clamp(rounded, 0.0, 1.0);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
new file mode 100644
index 0000000..50d42fc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.Objects;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Represents colors with 8 bits of precision.
+ */
+@SuppressWarnings("MemberName")
+public class Color8Bit {
+ public final int red;
+ public final int green;
+ public final int blue;
+
+ /**
+ * Constructs a Color8Bit.
+ *
+ * @param red Red value (0-255)
+ * @param green Green value (0-255)
+ * @param blue Blue value (0-255)
+ */
+ public Color8Bit(int red, int green, int blue) {
+ this.red = MathUtil.clamp(red, 0, 255);
+ this.green = MathUtil.clamp(green, 0, 255);
+ this.blue = MathUtil.clamp(blue, 0, 255);
+ }
+
+ /**
+ * Constructs a Color8Bit from a Color.
+ *
+ * @param color The color
+ */
+ public Color8Bit(Color color) {
+ this((int) (color.red * 255),
+ (int) (color.green * 255),
+ (int) (color.blue * 255));
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (this == other) {
+ return true;
+ }
+ if (other == null || getClass() != other.getClass()) {
+ return false;
+ }
+
+ Color8Bit color8Bit = (Color8Bit) other;
+ return red == color8Bit.red
+ && green == color8Bit.green
+ && blue == color8Bit.blue;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(red, green, blue);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
new file mode 100644
index 0000000..8bc86b2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Utility class for common WPILib error messages.
+ */
+public final class ErrorMessages {
+ /**
+ * Utility class, so constructor is private.
+ */
+ private ErrorMessages() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Requires that a parameter of a method not be null; prints an error message with
+ * helpful debugging instructions if the parameter is null.
+ *
+ * @param obj The parameter that must not be null.
+ * @param paramName The name of the parameter.
+ * @param methodName The name of the method.
+ */
+ public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
+ return requireNonNull(obj,
+ "Parameter " + paramName + " in method " + methodName + " was null when it"
+ + " should not have been! Check the stacktrace to find the responsible line of code - "
+ + "usually, it is the first line of user-written code indicated in the stacktrace. "
+ + "Make sure all objects passed to the method in question were properly initialized -"
+ + " note that this may not be obvious if it is being called under "
+ + "dynamically-changing conditions! Please do not seek additional technical assistance"
+ + " without doing this first!");
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java
new file mode 100644
index 0000000..1df7d33
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * Utility class that converts between commonly used units in FRC.
+ */
+public final class Units {
+ private static final double kInchesPerFoot = 12.0;
+ private static final double kMetersPerInch = 0.0254;
+ private static final double kSecondsPerMinute = 60;
+
+ /**
+ * Utility class, so constructor is private.
+ */
+ private Units() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Converts given meters to feet.
+ *
+ * @param meters The meters to convert to feet.
+ * @return Feet converted from meters.
+ */
+ public static double metersToFeet(double meters) {
+ return metersToInches(meters) / kInchesPerFoot;
+ }
+
+ /**
+ * Converts given feet to meters.
+ *
+ * @param feet The feet to convert to meters.
+ * @return Meters converted from feet.
+ */
+ public static double feetToMeters(double feet) {
+ return inchesToMeters(feet * kInchesPerFoot);
+ }
+
+ /**
+ * Converts given meters to inches.
+ *
+ * @param meters The meters to convert to inches.
+ * @return Inches converted from meters.
+ */
+ public static double metersToInches(double meters) {
+ return meters / kMetersPerInch;
+ }
+
+ /**
+ * Converts given inches to meters.
+ *
+ * @param inches The inches to convert to meters.
+ * @return Meters converted from inches.
+ */
+ public static double inchesToMeters(double inches) {
+ return inches * kMetersPerInch;
+ }
+
+ /**
+ * Converts given degrees to radians.
+ *
+ * @param degrees The degrees to convert to radians.
+ * @return Radians converted from degrees.
+ */
+ public static double degreesToRadians(double degrees) {
+ return Math.toRadians(degrees);
+ }
+
+ /**
+ * Converts given radians to degrees.
+ *
+ * @param radians The radians to convert to degrees.
+ * @return Degrees converted from radians.
+ */
+ public static double radiansToDegrees(double radians) {
+ return Math.toDegrees(radians);
+ }
+
+ /**
+ * Converts rotations per minute to radians per second.
+ *
+ * @param rpm The rotations per minute to convert to radians per second.
+ * @return Radians per second converted from rotations per minute.
+ */
+ public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
+ return rpm * Math.PI / (kSecondsPerMinute / 2);
+ }
+
+ /**
+ * Converts radians per second to rotations per minute.
+ *
+ * @param radiansPerSecond The radians per second to convert to from rotations per minute.
+ * @return Rotations per minute converted from radians per second.
+ */
+ public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
+ return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
new file mode 100644
index 0000000..2c0f8f2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.vision;
+
+import org.opencv.core.Mat;
+
+/**
+ * A vision pipeline is responsible for running a group of
+ * OpenCV algorithms to extract data from an image.
+ *
+ * @see VisionRunner
+ * @see VisionThread
+ *
+ * @deprecated Replaced with edu.wpi.first.vision.VisionPipeline
+ */
+@Deprecated
+public interface VisionPipeline {
+ /**
+ * Processes the image input and sets the result objects.
+ * Implementations should make these objects accessible.
+ */
+ void process(Mat image);
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
new file mode 100644
index 0000000..328686a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.vision;
+
+import org.opencv.core.Mat;
+
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+
+/**
+ * A vision runner is a convenient wrapper object to make it easy to run vision pipelines
+ * from robot code. The easiest way to use this is to run it in a {@link VisionThread}
+ * and use the listener to take snapshots of the pipeline's outputs.
+ *
+ * @see VisionPipeline
+ * @see VisionThread
+ * @see <a href="package-summary.html">vision</a>
+ *
+ * @deprecated Replaced with edu.wpi.first.vision.VisionRunner
+ */
+@Deprecated
+public class VisionRunner<P extends VisionPipeline> {
+ private final CvSink m_cvSink = new CvSink("VisionRunner CvSink");
+ private final P m_pipeline;
+ private final Mat m_image = new Mat();
+ private final Listener<? super P> m_listener;
+ private volatile boolean m_enabled = true;
+
+ /**
+ * Listener interface for a callback that should run after a pipeline has processed its input.
+ *
+ * @param <P> the type of the pipeline this listener is for
+ */
+ @FunctionalInterface
+ public interface Listener<P extends VisionPipeline> {
+ /**
+ * Called when the pipeline has run. This shouldn't take much time to run because it will delay
+ * later calls to the pipeline's {@link VisionPipeline#process process} method. Copying the
+ * outputs and code that uses the copies should be <i>synchronized</i> on the same mutex to
+ * prevent multiple threads from reading and writing to the same memory at the same time.
+ *
+ * @param pipeline the vision pipeline that ran
+ */
+ void copyPipelineOutputs(P pipeline);
+
+ }
+
+ /**
+ * Creates a new vision runner. It will take images from the {@code videoSource}, send them to
+ * the {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert
+ * user code when it is safe to access the pipeline's outputs.
+ *
+ * @param videoSource the video source to use to supply images for the pipeline
+ * @param pipeline the vision pipeline to run
+ * @param listener a function to call after the pipeline has finished running
+ */
+ public VisionRunner(VideoSource videoSource, P pipeline, Listener<? super P> listener) {
+ this.m_pipeline = pipeline;
+ this.m_listener = listener;
+ m_cvSink.setSource(videoSource);
+ }
+
+ /**
+ * Runs the pipeline one time, giving it the next image from the video source specified
+ * in the constructor. This will block until the source either has an image or throws an error.
+ * If the source successfully supplied a frame, the pipeline's image input will be set,
+ * the pipeline will run, and the listener specified in the constructor will be called to notify
+ * it that the pipeline ran.
+ *
+ * <p>This method is exposed to allow teams to add additional functionality or have their own
+ * ways to run the pipeline. Most teams, however, should just use {@link #runForever} in its own
+ * thread using a {@link VisionThread}.</p>
+ */
+ public void runOnce() {
+ Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId();
+
+ if (id != null && Thread.currentThread().getId() == id) {
+ throw new IllegalStateException(
+ "VisionRunner.runOnce() cannot be called from the main robot thread");
+ }
+ runOnceInternal();
+ }
+
+ private void runOnceInternal() {
+ long frameTime = m_cvSink.grabFrame(m_image);
+ if (frameTime == 0) {
+ // There was an error, report it
+ String error = m_cvSink.getError();
+ CameraServerSharedStore.getCameraServerShared().reportDriverStationError(error);
+ } else {
+ // No errors, process the image
+ m_pipeline.process(m_image);
+ m_listener.copyPipelineOutputs(m_pipeline);
+ }
+ }
+
+ /**
+ * A convenience method that calls {@link #runOnce()} in an infinite loop. This must
+ * be run in a dedicated thread, and cannot be used in the main robot thread because
+ * it will freeze the robot program.
+ *
+ * <p><strong>Do not call this method directly from the main thread.</strong></p>
+ *
+ * @throws IllegalStateException if this is called from the main robot thread
+ * @see VisionThread
+ */
+ public void runForever() {
+ Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId();
+
+ if (id != null && Thread.currentThread().getId() == id) {
+ throw new IllegalStateException(
+ "VisionRunner.runForever() cannot be called from the main robot thread");
+ }
+ while (m_enabled && !Thread.interrupted()) {
+ runOnceInternal();
+ }
+ }
+
+ /**
+ * Stop a RunForever() loop.
+ */
+ public void stop() {
+ m_enabled = false;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
new file mode 100644
index 0000000..5184ba0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.vision;
+
+import edu.wpi.cscore.VideoSource;
+
+/**
+ * A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread;
+ * it does not prevent the program from exiting when all other non-daemon threads
+ * have finished running.
+ *
+ * @see VisionPipeline
+ * @see VisionRunner
+ * @see Thread#setDaemon(boolean)
+ *
+ * @deprecated Replaced with edu.wpi.first.vision.VisionThread
+ */
+@Deprecated
+public class VisionThread extends Thread {
+ /**
+ * Creates a vision thread that continuously runs a {@link VisionPipeline}.
+ *
+ * @param visionRunner the runner for a vision pipeline
+ */
+ public VisionThread(VisionRunner<?> visionRunner) {
+ super(visionRunner::runForever, "WPILib Vision Thread");
+ setDaemon(true);
+ }
+
+ /**
+ * Creates a new vision thread that continuously runs the given vision pipeline. This is
+ * equivalent to {@code new VisionThread(new VisionRunner<>(videoSource, pipeline, listener))}.
+ *
+ * @param videoSource the source for images the pipeline should process
+ * @param pipeline the pipeline to run
+ * @param listener the listener to copy outputs from the pipeline after it runs
+ * @param <P> the type of the pipeline
+ */
+ public <P extends VisionPipeline> VisionThread(VideoSource videoSource,
+ P pipeline,
+ VisionRunner.Listener<? super P> listener) {
+ this(new VisionRunner<>(videoSource, pipeline, listener));
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
new file mode 100644
index 0000000..01c8d73
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+/**
+ * Classes in the {@code edu.wpi.first.wpilibj.vision} package are designed to
+ * simplify using OpenCV vision processing code from a robot program.
+ *
+ * <p>An example use case for grabbing a yellow tote from 2015 in autonomous:
+ * <br>
+ * <pre><code>
+ * public class Robot extends IterativeRobot
+ * implements VisionRunner.Listener<MyFindTotePipeline> {
+ *
+ * // A USB camera connected to the roboRIO.
+ * private {@link edu.wpi.cscore.VideoSource VideoSource} usbCamera;
+ *
+ * // A vision pipeline. This could be handwritten or generated by GRIP.
+ * // This has to implement {@link edu.wpi.first.wpilibj.vision.VisionPipeline}.
+ * // For this example, assume that it's perfect and will always see the tote.
+ * private MyFindTotePipeline findTotePipeline;
+ * private {@link edu.wpi.first.wpilibj.vision.VisionThread} findToteThread;
+ *
+ * // The object to synchronize on to make sure the vision thread doesn't
+ * // write to variables the main thread is using.
+ * private final Object visionLock = new Object();
+ *
+ * // The pipeline outputs we want
+ * private boolean pipelineRan = false; // lets us know when the pipeline has actually run
+ * private double angleToTote = 0;
+ * private double distanceToTote = 0;
+ *
+ * {@literal @}Override
+ * public void {@link edu.wpi.first.wpilibj.vision.VisionRunner.Listener#copyPipelineOutputs
+ * copyPipelineOutputs(MyFindTotePipeline pipeline)} {
+ * synchronized (visionLock) {
+ * // Take a snapshot of the pipeline's output because
+ * // it may have changed the next time this method is called!
+ * this.pipelineRan = true;
+ * this.angleToTote = pipeline.getAngleToTote();
+ * this.distanceToTote = pipeline.getDistanceToTote();
+ * }
+ * }
+ *
+ * {@literal @}Override
+ * public void robotInit() {
+ * usbCamera = CameraServer.getInstance().startAutomaticCapture(0);
+ * findTotePipeline = new MyFindTotePipeline();
+ * findToteThread = new VisionThread(usbCamera, findTotePipeline, this);
+ * }
+ *
+ * {@literal @}Override
+ * public void autonomousInit() {
+ * findToteThread.start();
+ * }
+ *
+ * {@literal @}Override
+ * public void autonomousPeriodic() {
+ * double angle;
+ * double distance;
+ * synchronized (visionLock) {
+ * if (!pipelineRan) {
+ * // Wait until the pipeline has run
+ * return;
+ * }
+ * // Copy the outputs to make sure they're all from the same run
+ * angle = this.angleToTote;
+ * distance = this.distanceToTote;
+ * }
+ * if (!aimedAtTote()) {
+ * turnToAngle(angle);
+ * } else if (!droveToTote()) {
+ * driveDistance(distance);
+ * } else if (!grabbedTote()) {
+ * grabTote();
+ * } else {
+ * // Tote was grabbed and we're done!
+ * return;
+ * }
+ * }
+ *
+ * }
+ * </code></pre>
+ */
+@java.lang.Deprecated
+package edu.wpi.first.wpilibj.vision;
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
new file mode 100644
index 0000000..13f1651
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class AddressableLEDBufferTest {
+ @ParameterizedTest
+ @MethodSource("hsvToRgbProvider")
+ @SuppressWarnings("ParameterName")
+ void hsvConvertTest(int h, int s, int v, int r, int g, int b) {
+ var buffer = new AddressableLEDBuffer(1);
+ buffer.setHSV(0, h, s, v);
+ assertAll(
+ () -> assertEquals((byte) r, buffer.m_buffer[2], "R value didn't match"),
+ () -> assertEquals((byte) g, buffer.m_buffer[1], "G value didn't match"),
+ () -> assertEquals((byte) b, buffer.m_buffer[0], "B value didn't match")
+ );
+ }
+
+ static Stream<Arguments> hsvToRgbProvider() {
+ return Stream.of(
+ arguments(0, 0, 0, 0, 0, 0), // Black
+ arguments(0, 0, 255, 255, 255, 255), // White
+ arguments(0, 255, 255, 255, 0, 0), // Red
+ arguments(60, 255, 255, 0, 255, 0), // Lime
+ arguments(120, 255, 255, 0, 0, 255), // Blue
+ arguments(30, 255, 255, 254, 255, 0), // Yellow (ish)
+ arguments(90, 255, 255, 0, 254, 255), // Cyan (ish)
+ arguments(150, 255, 255, 255, 0, 254), // Magenta (ish)
+ arguments(0, 0, 191, 191, 191, 191), // Silver
+ arguments(0, 0, 128, 128, 128, 128), // Gray
+ arguments(0, 255, 128, 128, 0, 0), // Maroon
+ arguments(30, 255, 128, 127, 128, 0), // Olive (ish)
+ arguments(60, 255, 128, 0, 128, 0), // Green
+ arguments(150, 255, 128, 128, 0, 127), // Purple (ish)
+ arguments(90, 255, 128, 0, 127, 128), // Teal (ish)
+ arguments(120, 255, 128, 0, 0, 128) // Navy
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
new file mode 100644
index 0000000..3953c4c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Random;
+import java.util.function.DoubleFunction;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class LinearFilterTest {
+ private static final double kFilterStep = 0.005;
+ private static final double kFilterTime = 2.0;
+ private static final double kSinglePoleIIRTimeConstant = 0.015915;
+ private static final double kHighPassTimeConstant = 0.006631;
+ private static final int kMovAvgTaps = 6;
+
+ private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+ private static final double kHighPassExpectedOutput = 10.074717;
+ private static final double kMovAvgExpectedOutput = -10.191644;
+
+ @SuppressWarnings("ParameterName")
+ private static double getData(double t) {
+ return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
+ }
+
+ @SuppressWarnings("ParameterName")
+ private static double getPulseData(double t) {
+ if (Math.abs(t - 1.0) < 0.001) {
+ return 1.0;
+ } else {
+ return 0.0;
+ }
+ }
+
+ @Test
+ void illegalTapNumberTest() {
+ assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
+ }
+
+ /**
+ * Test if the filter reduces the noise produced by a signal generator.
+ */
+ @ParameterizedTest
+ @MethodSource("noiseFilterProvider")
+ void noiseReduceTest(final LinearFilter filter) {
+ double noiseGenError = 0.0;
+ double filterError = 0.0;
+
+ final Random gen = new Random();
+ final double kStdDev = 10.0;
+
+ for (double t = 0; t < kFilterTime; t += kFilterStep) {
+ final double theory = getData(t);
+ final double noise = gen.nextGaussian() * kStdDev;
+ filterError += Math.abs(filter.calculate(theory + noise) - theory);
+ noiseGenError += Math.abs(noise - theory);
+ }
+
+ assertTrue(noiseGenError > filterError,
+ "Filter should have reduced noise accumulation from " + noiseGenError
+ + " but failed. The filter error was " + filterError);
+ }
+
+ static Stream<LinearFilter> noiseFilterProvider() {
+ return Stream.of(
+ LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+ LinearFilter.movingAverage(kMovAvgTaps)
+ );
+ }
+
+ /**
+ * Test if the linear filters produce consistent output for a given data set.
+ */
+ @ParameterizedTest
+ @MethodSource("outputFilterProvider")
+ void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
+ final double expectedOutput) {
+ double filterOutput = 0.0;
+ for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
+ filterOutput = filter.calculate(data.apply(t));
+ }
+
+ assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
+ }
+
+ static Stream<Arguments> outputFilterProvider() {
+ return Stream.of(
+ arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+ (DoubleFunction<Double>) LinearFilterTest::getData,
+ kSinglePoleIIRExpectedOutput),
+ arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
+ (DoubleFunction<Double>) LinearFilterTest::getData,
+ kHighPassExpectedOutput),
+ arguments(LinearFilter.movingAverage(kMovAvgTaps),
+ (DoubleFunction<Double>) LinearFilterTest::getData,
+ kMovAvgExpectedOutput),
+ arguments(LinearFilter.movingAverage(kMovAvgTaps),
+ (DoubleFunction<Double>) LinearFilterTest::getPulseData,
+ 0.0)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
new file mode 100644
index 0000000..00b100d
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class MedianFilterTest {
+ @Test
+ void medianFilterNotFullTestEven() {
+ MedianFilter filter = new MedianFilter(10);
+
+ filter.calculate(3);
+ filter.calculate(0);
+ filter.calculate(4);
+
+ assertEquals(3.5, filter.calculate(1000));
+ }
+
+ @Test
+ void medianFilterNotFullTestOdd() {
+ MedianFilter filter = new MedianFilter(10);
+
+ filter.calculate(3);
+ filter.calculate(0);
+ filter.calculate(4);
+ filter.calculate(7);
+
+ assertEquals(4, filter.calculate(1000));
+ }
+
+ @Test
+ void medianFilterFullTestEven() {
+ MedianFilter filter = new MedianFilter(6);
+
+ filter.calculate(3);
+ filter.calculate(0);
+ filter.calculate(0);
+ filter.calculate(5);
+ filter.calculate(4);
+ filter.calculate(1000);
+
+ assertEquals(4.5, filter.calculate(99));
+ }
+
+ @Test
+ void medianFilterFullTestOdd() {
+ MedianFilter filter = new MedianFilter(5);
+
+ filter.calculate(3);
+ filter.calculate(0);
+ filter.calculate(5);
+ filter.calculate(4);
+ filter.calculate(1000);
+
+ assertEquals(5, filter.calculate(99));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
new file mode 100644
index 0000000..d97b352
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.extension.BeforeAllCallback;
+import org.junit.jupiter.api.extension.ExtensionContext;
+import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.DriverStationSim;
+
+public final class MockHardwareExtension implements BeforeAllCallback {
+ private static ExtensionContext getRoot(ExtensionContext context) {
+ return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
+ }
+
+ @Override
+ public void beforeAll(ExtensionContext context) {
+ getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+ initializeHardware();
+ return true;
+ }, Boolean.class);
+ }
+
+ private void initializeHardware() {
+ HAL.initialize(500, 0);
+ DriverStationSim dsSim = new DriverStationSim();
+ dsSim.setDsAttached(true);
+ dsSim.setAutonomous(false);
+ dsSim.setEnabled(true);
+ dsSim.setTest(true);
+
+
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java
new file mode 100644
index 0000000..d183723
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+public class MockSpeedController implements SpeedController {
+ private double m_speed;
+ private boolean m_isInverted;
+
+ @Override
+ public void set(double speed) {
+ m_speed = m_isInverted ? -speed : speed;
+ }
+
+ @Override
+ public double get() {
+ return m_speed;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ @Override
+ public void disable() {
+ m_speed = 0;
+ }
+
+ @Override
+ public void stopMotor() {
+ disable();
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
new file mode 100644
index 0000000..7b3735f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Arrays;
+import java.util.Set;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Nested;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.io.TempDir;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+
+class PreferencesTest {
+ private final Preferences m_prefs = Preferences.getInstance();
+ private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("Preferences");
+
+ private static final String kFilename = "networktables.ini";
+
+ @BeforeAll
+ static void setupAll() {
+ NetworkTableInstance.getDefault().stopServer();
+ }
+
+ @BeforeEach
+ void setup(@TempDir Path tempDir) {
+ m_table.getKeys().forEach(m_table::delete);
+
+ Path filepath = tempDir.resolve(kFilename);
+ try (InputStream is = getClass().getResource("PreferencesTestDefault.ini").openStream()) {
+ Files.copy(is, filepath);
+ } catch (IOException ex) {
+ fail(ex);
+ }
+
+ NetworkTableInstance.getDefault().startServer(filepath.toString());
+ }
+
+ @AfterEach
+ void cleanup() {
+ NetworkTableInstance.getDefault().stopServer();
+ }
+
+ @AfterAll
+ static void cleanupAll() {
+ NetworkTableInstance.getDefault().startServer();
+ }
+
+ @Test
+ void removeAllTest() {
+ m_prefs.removeAll();
+
+ Set<String> keys = m_table.getKeys();
+ keys.remove(".type");
+
+ assertTrue(keys.isEmpty(), "Preferences was not empty! Preferences in table: "
+ + Arrays.toString(keys.toArray()));
+ }
+
+ @ParameterizedTest
+ @MethodSource("defaultKeyProvider")
+ void defaultKeysTest(String key) {
+ assertTrue(m_prefs.containsKey(key));
+ }
+
+ @ParameterizedTest
+ @MethodSource("defaultKeyProvider")
+ void defaultKeysAllTest(String key) {
+ assertTrue(m_prefs.getKeys().contains(key));
+ }
+
+ @Test
+ void defaultValueTest() {
+ assertAll(
+ () -> assertEquals(172L, m_prefs.getLong("checkedValueLong", 0)),
+ () -> assertEquals(0.2, m_prefs.getDouble("checkedValueDouble", 0), 1e-6),
+ () -> assertEquals("Hello. How are you?", m_prefs.getString("checkedValueString", "")),
+ () -> assertEquals(2, m_prefs.getInt("checkedValueInt", 0)),
+ () -> assertEquals(3.14, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
+ () -> assertFalse(m_prefs.getBoolean("checkedValueBoolean", true))
+ );
+ }
+
+ @Test
+ void backupTest() {
+ m_prefs.removeAll();
+
+ assertAll(
+ () -> assertEquals(0, m_prefs.getLong("checkedValueLong", 0)),
+ () -> assertEquals(0, m_prefs.getDouble("checkedValueDouble", 0), 1e-6),
+ () -> assertEquals("", m_prefs.getString("checkedValueString", "")),
+ () -> assertEquals(0, m_prefs.getInt("checkedValueInt", 0)),
+ () -> assertEquals(0, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
+ () -> assertTrue(m_prefs.getBoolean("checkedValueBoolean", true))
+ );
+ }
+
+ @Nested
+ class PutGetTests {
+ @Test
+ void intTest() {
+ final String key = "test";
+ final int value = 123;
+
+ m_prefs.putInt(key, value);
+
+ assertAll(
+ () -> assertEquals(value, m_prefs.getInt(key, -1)),
+ () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).intValue())
+ );
+ }
+
+ @Test
+ void longTest() {
+ final String key = "test";
+ final long value = 190L;
+
+ m_prefs.putLong(key, value);
+
+ assertAll(
+ () -> assertEquals(value, m_prefs.getLong(key, -1)),
+ () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).longValue())
+ );
+ }
+
+ @Test
+ void floatTest() {
+ final String key = "test";
+ final float value = 9.42f;
+
+ m_prefs.putFloat(key, value);
+
+ assertAll(
+ () -> assertEquals(value, m_prefs.getFloat(key, -1), 1e-6),
+ () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).floatValue(), 1e-6)
+ );
+ }
+
+ @Test
+ void doubleTest() {
+ final String key = "test";
+ final double value = 6.28;
+
+ m_prefs.putDouble(key, value);
+
+ assertAll(
+ () -> assertEquals(value, m_prefs.getDouble(key, -1), 1e-6),
+ () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).doubleValue(), 1e-6)
+ );
+ }
+
+ @Test
+ void stringTest() {
+ final String key = "test";
+ final String value = "value";
+
+ m_prefs.putString(key, value);
+
+ assertAll(
+ () -> assertEquals(value, m_prefs.getString(key, "")),
+ () -> assertEquals(value, m_table.getEntry(key).getString(""))
+ );
+ }
+
+ @Test
+ void booleanTest() {
+ final String key = "test";
+ final boolean value = true;
+
+ m_prefs.putBoolean(key, value);
+
+ assertAll(
+ () -> assertEquals(value, m_prefs.getBoolean(key, false)),
+ () -> assertEquals(value, m_table.getEntry(key).getBoolean(false))
+ );
+ }
+ }
+
+ static Stream<String> defaultKeyProvider() {
+ return Stream.of(
+ "checkedValueLong",
+ "checkedValueDouble",
+ "checkedValueString",
+ "checkedValueInt",
+ "checkedValueFloat",
+ "checkedValueBoolean"
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
new file mode 100644
index 0000000..4bce18f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+class RobotControllerTest extends UtilityClassTest<RobotController> {
+ RobotControllerTest() {
+ super(RobotController.class);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
new file mode 100644
index 0000000..e47b70b
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class SlewRateLimiterTest {
+ @Test
+ void slewRateLimitTest() {
+ SlewRateLimiter limiter = new SlewRateLimiter(1);
+ Timer.delay(1);
+ assertTrue(limiter.calculate(2) < 2);
+ }
+
+ @Test
+ void slewRateNoLimitTest() {
+ SlewRateLimiter limiter = new SlewRateLimiter(1);
+ Timer.delay(1);
+ assertEquals(limiter.calculate(0.5), 0.5);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java
new file mode 100644
index 0000000..62e2f46
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Arrays;
+import java.util.stream.DoubleStream;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class SpeedControllerGroupTest {
+ private static Stream<Arguments> speedControllerArguments() {
+ return IntStream.of(1, 2, 3).mapToObj(number -> {
+ SpeedController[] speedControllers = Stream.generate(MockSpeedController::new)
+ .limit(number)
+ .toArray(SpeedController[]::new);
+ SpeedControllerGroup group = new SpeedControllerGroup(speedControllers[0],
+ Arrays.copyOfRange(speedControllers, 1, speedControllers.length));
+ return Arguments.of(group, speedControllers);
+ });
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void setTest(final SpeedControllerGroup group, final SpeedController[] speedControllers) {
+ group.set(1.0);
+
+ assertArrayEquals(DoubleStream.generate(() -> 1.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void getInvertedTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.setInverted(true);
+
+ assertTrue(group.getInverted());
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void setInvertedDoesNotModifySpeedControllersTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.setInverted(true);
+
+ assertArrayEquals(Stream.generate(() -> false).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).map(SpeedController::getInverted).toArray());
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void setInvertedDoesInvertTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.setInverted(true);
+ group.set(1.0);
+
+ assertArrayEquals(DoubleStream.generate(() -> -1.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void disableTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.set(1.0);
+ group.disable();
+
+ assertArrayEquals(DoubleStream.generate(() -> 0.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void stopMotorTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.set(1.0);
+ group.stopMotor();
+
+ assertArrayEquals(DoubleStream.generate(() -> 0.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void pidWriteTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.pidWrite(1.0);
+
+ assertArrayEquals(DoubleStream.generate(() -> 1.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
new file mode 100644
index 0000000..a66cd13
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.DynamicTest.dynamicTest;
+
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class UtilityClassTest<T> {
+ private final Class<T> m_clazz;
+
+ protected UtilityClassTest(Class<T> clazz) {
+ m_clazz = clazz;
+ }
+
+ @Test
+ public void singleConstructorTest() {
+ assertEquals(1, m_clazz.getDeclaredConstructors().length,
+ "More than one constructor defined");
+ }
+
+ @Test
+ public void constructorPrivateTest() {
+ Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+
+ assertFalse(constructor.canAccess(null), "Constructor is not private");
+ }
+
+ @Test
+ public void constructorReflectionTest() {
+ Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+ constructor.setAccessible(true);
+ assertThrows(InvocationTargetException.class, constructor::newInstance);
+ }
+
+ @TestFactory
+ Stream<DynamicTest> publicMethodsStaticTestFactory() {
+ return Arrays.stream(m_clazz.getDeclaredMethods())
+ .filter(method -> Modifier.isPublic(method.getModifiers()))
+ .map(method -> dynamicTest(method.getName(),
+ () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
new file mode 100644
index 0000000..b956446
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.condition.DisabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@DisabledOnOs(OS.MAC)
+class WatchdogTest {
+ @Test
+ void enableDisableTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1));
+
+ System.out.println("Run 1");
+ watchdog.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ System.out.println("Run 2");
+ watchdogCounter.set(0);
+ watchdog.enable();
+ try {
+ Thread.sleep(600);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(1, watchdogCounter.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+
+ // Run 3
+ watchdogCounter.set(0);
+ watchdog.enable();
+ try {
+ Thread.sleep(1000);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(1, watchdogCounter.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+
+ watchdog.close();
+ }
+
+ @Test
+ void resetTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1));
+
+ watchdog.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.reset();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ watchdog.close();
+ }
+
+ @Test
+ void setTimeoutTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(1.0, () -> watchdogCounter.addAndGet(1));
+
+ watchdog.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.setTimeout(0.2);
+
+ assertEquals(0.2, watchdog.getTimeout());
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ try {
+ Thread.sleep(300);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(1, watchdogCounter.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+
+ watchdog.close();
+ }
+
+ @Test
+ void isExpiredTest() {
+ final Watchdog watchdog = new Watchdog(0.2, () -> {
+ });
+ assertFalse(watchdog.isExpired());
+ watchdog.enable();
+
+ assertFalse(watchdog.isExpired());
+ try {
+ Thread.sleep(300);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ assertTrue(watchdog.isExpired());
+
+ watchdog.disable();
+ assertTrue(watchdog.isExpired());
+
+ watchdog.reset();
+ assertFalse(watchdog.isExpired());
+
+ watchdog.close();
+ }
+
+ @Test
+ void epochsTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1));
+
+ System.out.println("Run 1");
+ watchdog.enable();
+ watchdog.addEpoch("Epoch 1");
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.addEpoch("Epoch 2");
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.addEpoch("Epoch 3");
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ System.out.println("Run 2");
+ watchdog.enable();
+ watchdog.addEpoch("Epoch 1");
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.reset();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.addEpoch("Epoch 2");
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ watchdog.close();
+ }
+
+ @Test
+ void multiWatchdogTest() {
+ final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
+ final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
+
+ final Watchdog watchdog1 = new Watchdog(0.2, () -> watchdogCounter1.addAndGet(1));
+ final Watchdog watchdog2 = new Watchdog(0.6, () -> watchdogCounter2.addAndGet(1));
+
+ watchdog2.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
+ assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
+
+ // Sleep enough such that only the watchdog enabled later times out first
+ watchdog1.enable();
+ try {
+ Thread.sleep(300);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog1.disable();
+ watchdog2.disable();
+
+ assertEquals(1, watchdogCounter1.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+ assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
+
+ watchdog1.close();
+ watchdog2.close();
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
new file mode 100644
index 0000000..e804d0c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.can;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.can.CANJNI;
+import edu.wpi.first.hal.can.CANStatus;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+class CANStatusTest {
+ @Test
+ void canStatusGetDoesntThrow() {
+ HAL.initialize(500, 0);
+ CANStatus status = new CANStatus();
+ assertDoesNotThrow(() -> CANJNI.GetCANStatus(status));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
new file mode 100644
index 0000000..873d03f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PIDInputOutputTest {
+ private PIDController m_controller;
+
+ @BeforeEach
+ void setUp() {
+ m_controller = new PIDController(0, 0, 0);
+ }
+
+ @Test
+ void continuousInputTest() {
+ m_controller.setP(1);
+ m_controller.enableContinuousInput(-180, 180);
+
+ assertTrue(m_controller.calculate(-179, 179) < 0.0);
+ }
+
+ @Test
+ void proportionalGainOutputTest() {
+ m_controller.setP(4);
+
+ assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+ }
+
+ @Test
+ void integralGainOutputTest() {
+ m_controller.setI(4);
+
+ double out = 0;
+
+ for (int i = 0; i < 5; i++) {
+ out = m_controller.calculate(0.025, 0);
+ }
+
+ assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+ }
+
+ @Test
+ void derivativeGainOutputTest() {
+ m_controller.setD(4);
+
+ m_controller.calculate(0, 0);
+
+ assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
new file mode 100644
index 0000000..cd2aba4
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PIDToleranceTest {
+ private static final double kSetpoint = 50.0;
+ private static final double kTolerance = 10.0;
+ private static final double kRange = 200;
+
+ @Test
+ void initialToleranceTest() {
+ var controller = new PIDController(0.05, 0.0, 0.0);
+ controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+ assertTrue(controller.atSetpoint());
+ }
+
+ @Test
+ void absoluteToleranceTest() {
+ var controller = new PIDController(0.05, 0.0, 0.0);
+ controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+ controller.setTolerance(kTolerance);
+ controller.setSetpoint(kSetpoint);
+
+ controller.calculate(0.0);
+
+ assertFalse(controller.atSetpoint(),
+ "Error was in tolerance when it should not have been. Error was " + controller
+ .getPositionError());
+
+ controller.calculate(kSetpoint + kTolerance / 2);
+
+ assertTrue(controller.atSetpoint(),
+ "Error was not in tolerance when it should have been. Error was " + controller
+ .getPositionError());
+
+ controller.calculate(kSetpoint + 10 * kTolerance);
+
+ assertFalse(controller.atSetpoint(),
+ "Error was in tolerance when it should not have been. Error was " + controller
+ .getPositionError());
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
new file mode 100644
index 0000000..6babcc2
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ProfiledPIDControllerTest {
+ @Test
+ void testStartFromNonZeroPosition() {
+ ProfiledPIDController controller = new ProfiledPIDController(1.0, 0.0, 0.0,
+ new TrapezoidProfile.Constraints(1.0, 1.0));
+
+ controller.reset(20);
+
+ assertEquals(0.0, controller.calculate(20), 0.05);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
new file mode 100644
index 0000000..2bef50d
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class RamseteControllerTest {
+ private static final double kTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = Math.toRadians(2);
+
+ private static double boundRadians(double value) {
+ while (value > Math.PI) {
+ value -= Math.PI * 2;
+ }
+ while (value <= -Math.PI) {
+ value += Math.PI * 2;
+ }
+ return value;
+ }
+
+ @Test
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ void testReachesReference() {
+ final var controller = new RamseteController(2.0, 0.7);
+ var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+ waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ final double kDt = 0.02;
+ final var totalTime = trajectory.getTotalTimeSeconds();
+ for (int i = 0; i < (totalTime / kDt); ++i) {
+ var state = trajectory.sample(kDt * i);
+
+ var output = controller.calculate(robotPose, state);
+ robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt, 0,
+ output.omegaRadiansPerSecond * kDt));
+ }
+
+ final var states = trajectory.getStates();
+ final var endPose = states.get(states.size() - 1).poseMeters;
+
+ // Java lambdas require local variables referenced from a lambda expression
+ // must be final or effectively final.
+ final var finalRobotPose = robotPose;
+ assertAll(
+ () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(),
+ kTolerance),
+ () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(),
+ kTolerance),
+ () -> assertEquals(0.0,
+ boundRadians(endPose.getRotation().getRadians()
+ - finalRobotPose.getRotation().getRadians()),
+ kAngularTolerance)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java
new file mode 100644
index 0000000..14e9401
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java
@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.MockSpeedController;
+import edu.wpi.first.wpilibj.RobotDrive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Tests DifferentialDrive and MecanumDrive.
+ */
+public class DriveTest {
+ private final MockSpeedController m_rdFrontLeft = new MockSpeedController();
+ private final MockSpeedController m_rdRearLeft = new MockSpeedController();
+ private final MockSpeedController m_rdFrontRight = new MockSpeedController();
+ private final MockSpeedController m_rdRearRight = new MockSpeedController();
+ private final MockSpeedController m_frontLeft = new MockSpeedController();
+ private final MockSpeedController m_rearLeft = new MockSpeedController();
+ private final MockSpeedController m_frontRight = new MockSpeedController();
+ private final MockSpeedController m_rearRight = new MockSpeedController();
+ private final RobotDrive m_robotDrive =
+ new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight);
+ private final DifferentialDrive m_differentialDrive =
+ new DifferentialDrive(m_frontLeft, m_frontRight);
+ private final MecanumDrive m_mecanumDrive =
+ new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
+
+ private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9,
+ -1.0};
+ private final double[] m_testGyroValues = {0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540,
+ -45, -90, -135, -180, -225, -270, -305, -360, -540};
+
+ @BeforeEach
+ void setUp() {
+ m_differentialDrive.setDeadband(0.0);
+ m_differentialDrive.setSafetyEnabled(false);
+ m_mecanumDrive.setDeadband(0.0);
+ m_mecanumDrive.setSafetyEnabled(false);
+ m_robotDrive.setSafetyEnabled(false);
+ }
+
+ @Test
+ public void testTankDriveSquared() {
+ for (double leftJoystick : m_testJoystickValues) {
+ for (double rightJoystick : m_testJoystickValues) {
+ m_robotDrive.tankDrive(leftJoystick, rightJoystick);
+ m_differentialDrive.tankDrive(leftJoystick, rightJoystick);
+ assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+ "Left Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+ + rightJoystick);
+ assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+ "Right Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+ + rightJoystick);
+ }
+ }
+ }
+
+ @Test
+ void testTankDrive() {
+ for (double leftJoystick : m_testJoystickValues) {
+ for (double rightJoystick : m_testJoystickValues) {
+ m_robotDrive.tankDrive(leftJoystick, rightJoystick, false);
+ m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false);
+ assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+ "Left Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+ + rightJoystick);
+ assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+ "Right Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+ + rightJoystick);
+ }
+ }
+ }
+
+ @Test
+ void testArcadeDriveSquared() {
+ for (double moveJoystick : m_testJoystickValues) {
+ for (double rotateJoystick : m_testJoystickValues) {
+ m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick);
+ m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick);
+ assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+ "Left Motor squared didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
+ + rotateJoystick);
+ assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+ "Right Motor squared didn't match. Move Joystick: " + moveJoystick
+ + " Rotate Joystick: " + rotateJoystick);
+ }
+ }
+ }
+
+ @Test
+ void testArcadeDrive() {
+ for (double moveJoystick : m_testJoystickValues) {
+ for (double rotateJoystick : m_testJoystickValues) {
+ m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false);
+ m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false);
+ assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+ "Left Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
+ + rotateJoystick);
+ assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+ "Right Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
+ + rotateJoystick);
+ }
+ }
+ }
+
+ @Test
+ void testMecanumPolar() {
+ for (double magnitudeJoystick : m_testJoystickValues) {
+ for (double directionJoystick : m_testGyroValues) {
+ for (double rotationJoystick : m_testJoystickValues) {
+ m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick);
+ m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick);
+ assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+ "Left Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ + rotationJoystick);
+ assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01,
+ "Right Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ + rotationJoystick);
+ assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01,
+ "Left Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ + rotationJoystick);
+ assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01,
+ "Right Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+ + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+ + rotationJoystick);
+ }
+ }
+ }
+ }
+
+ @Test
+ @SuppressWarnings("checkstyle:LocalVariableName")
+ void testMecanumCartesian() {
+ for (double x_Joystick : m_testJoystickValues) {
+ for (double y_Joystick : m_testJoystickValues) {
+ for (double rotationJoystick : m_testJoystickValues) {
+ for (double gyroValue : m_testGyroValues) {
+ m_robotDrive.mecanumDrive_Cartesian(x_Joystick, y_Joystick, rotationJoystick,
+ gyroValue);
+ m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue);
+ assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+ "Left Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+ + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+ assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01,
+ "Right Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+ + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+ assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01,
+ "Left Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+ + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+ assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01,
+ "Right Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+ + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+ }
+ }
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
new file mode 100644
index 0000000..066716e
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Pose2dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testTransformBy() {
+ var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+ var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+ Rotation2d.fromDegrees(5.0));
+
+ var transformed = initial.plus(transformation);
+
+ assertAll(
+ () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
+ () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
+ () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testRelativeTo() {
+ var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+ var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+ var finalRelativeToInitial = last.relativeTo(initial);
+
+ assertAll(
+ () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0),
+ kEpsilon),
+ () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon),
+ () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testEquality() {
+ var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+ var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+ var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
+ assertNotEquals(one, two);
+ }
+
+ void testMinus() {
+ var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+ var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+ final var transform = last.minus(initial);
+
+ assertAll(
+ () -> assertEquals(transform.getTranslation().getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
+ () -> assertEquals(transform.getTranslation().getY(), 0.0, kEpsilon),
+ () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
new file mode 100644
index 0000000..6c4b1b3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Rotation2dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testRadiansToDegrees() {
+ var one = new Rotation2d(Math.PI / 3);
+ var two = new Rotation2d(Math.PI / 4);
+
+ assertAll(
+ () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
+ () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testRadiansAndDegrees() {
+ var one = Rotation2d.fromDegrees(45.0);
+ var two = Rotation2d.fromDegrees(30.0);
+
+ assertAll(
+ () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
+ () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
+ );
+ }
+
+ @Test
+ void testRotateByFromZero() {
+ var zero = new Rotation2d();
+ var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
+
+ assertAll(
+ () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
+ () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testRotateByNonZero() {
+ var rot = Rotation2d.fromDegrees(90.0);
+ rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+ assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+ }
+
+ @Test
+ void testMinus() {
+ var one = Rotation2d.fromDegrees(70.0);
+ var two = Rotation2d.fromDegrees(30.0);
+
+ assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
+ }
+
+ @Test
+ void testEquality() {
+ var one = Rotation2d.fromDegrees(43.0);
+ var two = Rotation2d.fromDegrees(43.0);
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var one = Rotation2d.fromDegrees(43.0);
+ var two = Rotation2d.fromDegrees(43.5);
+ assertNotEquals(one, two);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
new file mode 100644
index 0000000..b4e2947
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Translation2dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testSum() {
+ var one = new Translation2d(1.0, 3.0);
+ var two = new Translation2d(2.0, 5.0);
+
+ var sum = one.plus(two);
+
+ assertAll(
+ () -> assertEquals(sum.getX(), 3.0, kEpsilon),
+ () -> assertEquals(sum.getY(), 8.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testDifference() {
+ var one = new Translation2d(1.0, 3.0);
+ var two = new Translation2d(2.0, 5.0);
+
+ var difference = one.minus(two);
+
+ assertAll(
+ () -> assertEquals(difference.getX(), -1.0, kEpsilon),
+ () -> assertEquals(difference.getY(), -2.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testRotateBy() {
+ var another = new Translation2d(3.0, 0.0);
+ var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
+
+ assertAll(
+ () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
+ () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testMultiplication() {
+ var original = new Translation2d(3.0, 5.0);
+ var mult = original.times(3);
+
+ assertAll(
+ () -> assertEquals(mult.getX(), 9.0, kEpsilon),
+ () -> assertEquals(mult.getY(), 15.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testDivision() {
+ var original = new Translation2d(3.0, 5.0);
+ var div = original.div(2);
+
+ assertAll(
+ () -> assertEquals(div.getX(), 1.5, kEpsilon),
+ () -> assertEquals(div.getY(), 2.5, kEpsilon)
+ );
+ }
+
+ @Test
+ void testNorm() {
+ var one = new Translation2d(3.0, 5.0);
+ assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+ }
+
+ @Test
+ void testDistance() {
+ var one = new Translation2d(1, 1);
+ var two = new Translation2d(6, 6);
+ assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+ }
+
+ @Test
+ void testUnaryMinus() {
+ var original = new Translation2d(-4.5, 7);
+ var inverted = original.unaryMinus();
+
+ assertAll(
+ () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
+ () -> assertEquals(inverted.getY(), -7, kEpsilon)
+ );
+ }
+
+ @Test
+ void testEquality() {
+ var one = new Translation2d(9, 5.5);
+ var two = new Translation2d(9, 5.5);
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var one = new Translation2d(9, 5.5);
+ var two = new Translation2d(9, 5.7);
+ assertNotEquals(one, two);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
new file mode 100644
index 0000000..903c436
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Twist2dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testStraightLineTwist() {
+ var straight = new Twist2d(5.0, 0.0, 0.0);
+ var straightPose = new Pose2d().exp(straight);
+
+ assertAll(
+ () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon),
+ () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon),
+ () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testQuarterCirleTwist() {
+ var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+ var quarterCirclePose = new Pose2d().exp(quarterCircle);
+
+ assertAll(
+ () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon),
+ () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon),
+ () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testDiagonalNoDtheta() {
+ var diagonal = new Twist2d(2.0, 2.0, 0.0);
+ var diagonalPose = new Pose2d().exp(diagonal);
+
+ assertAll(
+ () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon),
+ () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon),
+ () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
+ );
+ }
+
+ @Test
+ void testEquality() {
+ var one = new Twist2d(5, 1, 3);
+ var two = new Twist2d(5, 1, 3);
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var one = new Twist2d(5, 1, 3);
+ var two = new Twist2d(5, 1.2, 3);
+ assertNotEquals(one, two);
+ }
+
+ void testPose2dLog() {
+ final var start = new Pose2d();
+ final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
+
+ final var twist = start.log(end);
+
+ assertAll(
+ () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
+ () -> assertEquals(twist.dy, 0.0, kEpsilon),
+ () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
new file mode 100644
index 0000000..f70f3f8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.hal;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+
+class JNITest {
+ @Test
+ void jniNtcoreLinkTest() {
+ // Test to verify that the JNI test link works correctly.
+ NetworkTablesJNI.flush(NetworkTablesJNI.getDefaultInstance());
+ }
+
+ @Test
+ void jniHalLinkTest() {
+ HAL.initialize(500, 0);
+ // Test to verify that the JNI test link works correctly.
+ HALUtil.getHALRuntimeType();
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
new file mode 100644
index 0000000..98b8489
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.hal;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.hal.sim.mockdata.DriverStationDataJNI;
+import edu.wpi.first.wpilibj.DriverStation.MatchType;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MatchInfoDataTest {
+ @Test
+ void testSetMatchInfo() {
+
+ MatchType matchType = MatchType.Qualification;
+ DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal());
+
+ MatchInfoData outMatchInfo = new MatchInfoData();
+ HAL.getMatchInfo(outMatchInfo);
+
+ assertAll(
+ () -> assertEquals("Event Name", outMatchInfo.eventName),
+ () -> assertEquals(matchType.ordinal(), outMatchInfo.matchType),
+ () -> assertEquals(174, outMatchInfo.matchNumber),
+ () -> assertEquals(191, outMatchInfo.replayNumber),
+ () -> assertEquals("Game Message", outMatchInfo.gameSpecificMessage)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
new file mode 100644
index 0000000..729d7b8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ChassisSpeedsTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testFieldRelativeConstruction() {
+ final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+ 1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
+ );
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
new file mode 100644
index 0000000..e484eab
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class DifferentialDriveKinematicsTest {
+ private static final double kEpsilon = 1E-9;
+ private final DifferentialDriveKinematics m_kinematics
+ = new DifferentialDriveKinematics(0.381 * 2);
+
+ @Test
+ void testInverseKinematicsForZeros() {
+ var chassisSpeeds = new ChassisSpeeds();
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+ assertAll(
+ () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testForwardKinematicsForZeros() {
+ var wheelSpeeds = new DifferentialDriveWheelSpeeds();
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testInverseKinematicsForStraightLine() {
+ var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+ assertAll(
+ () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+ () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testForwardKinematicsForStraightLine() {
+ var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testInverseKinematicsForRotateInPlace() {
+ var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
+ var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+ assertAll(
+ () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+ () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testForwardKinematicsForRotateInPlace() {
+ var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
new file mode 100644
index 0000000..aabc455
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class DifferentialDriveOdometryTest {
+ private static final double kEpsilon = 1E-9;
+ private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
+ new Rotation2d());
+
+ @Test
+ void testOdometryWithEncoderDistances() {
+ m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+ var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
+
+ assertAll(
+ () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon),
+ () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon),
+ () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
new file mode 100644
index 0000000..75b6f44
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class MecanumDriveKinematicsTest {
+ private static final double kEpsilon = 1E-9;
+
+ private final Translation2d m_fl = new Translation2d(12, 12);
+ private final Translation2d m_fr = new Translation2d(12, -12);
+ private final Translation2d m_bl = new Translation2d(-12, 12);
+ private final Translation2d m_br = new Translation2d(-12, -12);
+
+ private final MecanumDriveKinematics m_kinematics =
+ new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+ @Test
+ void testStraightLineInverseKinematics() {
+ ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+ var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
+ */
+
+ assertAll(
+ () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
+ () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
+ () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
+ () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testStraightLineForwardKinematicsKinematics() {
+
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+ var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
+ */
+
+ assertAll(
+ () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
+ () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+ () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testStrafeInverseKinematics() {
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
+ var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
+ */
+
+ assertAll(
+ () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
+ () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
+ () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
+ () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testStrafeForwardKinematicsKinematics() {
+
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
+ var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
+ */
+
+ assertAll(
+ () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+ () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
+ () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testRotationInverseKinematics() {
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+ var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
+ */
+
+ assertAll(
+ () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
+ () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
+ () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
+ () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testRotationForwardKinematicsKinematics() {
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191);
+ var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should be [[0][0][2pi]]
+ */
+
+ assertAll(
+ () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+ () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+ () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testMixedTranslationRotationInverseKinematics() {
+ ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
+ var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
+ */
+
+ assertAll(
+ () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
+ () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
+ () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
+ () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testMixedTranslationRotationForwardKinematicsKinematics() {
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
+ var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be [[2][3][1]]
+ */
+
+ assertAll(
+ () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
+ () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
+ () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testOffCenterRotationInverseKinematics() {
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
+ var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
+ */
+
+ assertAll(
+ () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
+ () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
+ () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
+ () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testOffCenterRotationForwardKinematicsKinematics() {
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
+ var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from the wheel
+ velocities should be [[12][-12][1]]
+ */
+
+ assertAll(
+ () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
+ () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
+ () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testOffCenterTranslationRotationInverseKinematics() {
+ ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
+ var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
+ */
+
+ assertAll(
+ () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
+ () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
+ () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
+ () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testOffCenterRotationTranslationForwardKinematicsKinematics() {
+
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
+ var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from the wheel
+ velocities should be [[17][-10][1]]
+ */
+
+ assertAll(
+ () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
+ () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
+ () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testNormalize() {
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
+ wheelSpeeds.normalize(5.5);
+
+ double factor = 5.5 / 7.0;
+
+ assertAll(
+ () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
+ () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
+ () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
+ () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
+ );
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
new file mode 100644
index 0000000..4619bf0
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumDriveOdometryTest {
+ private final Translation2d m_fl = new Translation2d(12, 12);
+ private final Translation2d m_fr = new Translation2d(12, -12);
+ private final Translation2d m_bl = new Translation2d(-12, 12);
+ private final Translation2d m_br = new Translation2d(-12, -12);
+
+
+ private final MecanumDriveKinematics m_kinematics =
+ new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+ new Rotation2d());
+
+ @Test
+ void testMultipleConsecutiveUpdates() {
+ var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+ m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+ var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(secondPose.getTranslation().getX(), 0.0, 0.01),
+ () -> assertEquals(secondPose.getTranslation().getY(), 0.0, 0.01),
+ () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
+ );
+ }
+
+ @Test
+ void testTwoIterations() {
+ // 5 units/sec in the x axis (forward)
+ final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+ m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+ var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01),
+ () -> assertEquals(0, pose.getTranslation().getY(), 0.01),
+ () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
+ );
+ }
+
+ @Test
+ void test90degreeTurn() {
+ // This is a 90 degree turn about the point between front left and rear left wheels
+ // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
+ final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+
+ m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+ final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01),
+ () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
+ () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
+ );
+ }
+
+ @Test
+ void testGyroAngleReset() {
+ var gyro = Rotation2d.fromDegrees(90.0);
+ var fieldAngle = Rotation2d.fromDegrees(0.0);
+ m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+ var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
+ 3.536, 3.536);
+ m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
+ var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+
+ assertAll(
+ () -> assertEquals(5.0, pose.getTranslation().getX(), 0.1),
+ () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
+ () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+ );
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
new file mode 100644
index 0000000..f506436
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class SwerveDriveKinematicsTest {
+ private static final double kEpsilon = 1E-9;
+
+ private final Translation2d m_fl = new Translation2d(12, 12);
+ private final Translation2d m_fr = new Translation2d(12, -12);
+ private final Translation2d m_bl = new Translation2d(-12, 12);
+ private final Translation2d m_br = new Translation2d(-12, -12);
+
+ private final SwerveDriveKinematics m_kinematics =
+ new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+ @Test
+ void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
+
+ ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+ var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+ assertAll(
+ () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
+ () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
+ () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
+ () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)
+ );
+ }
+
+ @Test
+ void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
+ SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testStraightStrafeInverseKinematics() {
+
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
+ var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+ assertAll(
+ () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+ );
+ }
+
+ @Test
+ void testStraightStrafeForwardKinematics() {
+ SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+ );
+ }
+
+ @Test
+ void testTurnInPlaceInverseKinematics() {
+
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+ var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+ /*
+ The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
+ the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
+ trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
+ our wheels must trace out 1 rotation (or 106.63 inches) per second.
+ */
+
+ assertAll(
+ () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
+ () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
+ () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
+ () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
+ () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+ );
+ }
+
+ @Test
+ void testTurnInPlaceForwardKinematics() {
+ SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
+ SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
+ SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
+ SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
+
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testOffCenterCORRotationInverseKinematics() {
+
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+ var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
+
+ /*
+ This one is a bit trickier. Because we are rotating about the front-left wheel,
+ it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
+ an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
+ radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
+ should be pointing straight forward, the back-left wheel should be pointing straight right,
+ and the back-right wheel should be at a -45 degree angle
+ */
+
+ assertAll(
+ () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
+ () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
+ () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
+ () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
+ () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+ );
+ }
+
+ @Test
+ void testOffCenterCORRotationForwardKinematics() {
+ SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
+ SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
+ SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
+ SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
+
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+ /*
+ We already know that our omega should be 2pi from the previous test. Next, we need to determine
+ the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+ we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+ a full revolution about the center of revolution once every second. Therefore, the center of
+ mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
+ 1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+ */
+
+ assertAll(
+ () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
+ () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
+ () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
+ SwerveModuleState tolerance) {
+ assertAll(
+ () -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
+ tolerance.speedMetersPerSecond),
+ () -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
+ tolerance.angle.getDegrees())
+ );
+ }
+
+ /**
+ * Test the rotation of the robot about a non-central point with
+ * both linear and angular velocities.
+ */
+ @Test
+ void testOffCenterCORRotationAndTranslationInverseKinematics() {
+
+ ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
+ var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
+
+ // By equation (13.14) from state-space guide, our wheels/angles will be as follows,
+ // (+-1 degree or speed):
+ SwerveModuleState[] expectedStates = new SwerveModuleState[]{
+ new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
+ new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
+ new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
+ new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
+ };
+ var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
+
+ for (int i = 0; i < expectedStates.length; i++) {
+ assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
+ }
+ }
+
+ @Test
+ void testOffCenterCORRotationAndTranslationForwardKinematics() {
+ SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
+ SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
+ SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
+ SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
+
+ var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+ /*
+ From equation (13.17), we know that chassis motion is th dot product of the
+ pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+ (0,0) -- we don't want the motion of the center of rotation, we want it of
+ the center of the robot). These above SwerveModuleStates are known to be from
+ a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+ calculated using Numpy's linalg.pinv function.
+ */
+
+ assertAll(
+ () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
+ () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
+ () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+ );
+ }
+
+ @Test
+ void testNormalize() {
+ SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
+ SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
+ SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
+ SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
+
+ SwerveModuleState[] arr = {fl, fr, bl, br};
+ SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
+
+ double factor = 5.5 / 7.0;
+
+ assertAll(
+ () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
+ );
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
new file mode 100644
index 0000000..85b832c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveDriveOdometryTest {
+ private final Translation2d m_fl = new Translation2d(12, 12);
+ private final Translation2d m_fr = new Translation2d(12, -12);
+ private final Translation2d m_bl = new Translation2d(-12, 12);
+ private final Translation2d m_br = new Translation2d(-12, -12);
+
+ private final SwerveDriveKinematics m_kinematics =
+ new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+ new Rotation2d());
+
+ @Test
+ void testTwoIterations() {
+ // 5 units/sec in the x axis (forward)
+ final SwerveModuleState[] wheelSpeeds = {
+ new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+ new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+ new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+ new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+ };
+
+ m_odometry.updateWithTime(0.0, new Rotation2d(),
+ new SwerveModuleState(), new SwerveModuleState(),
+ new SwerveModuleState(), new SwerveModuleState());
+ var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01),
+ () -> assertEquals(0, pose.getTranslation().getY(), 0.01),
+ () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
+ );
+ }
+
+ @Test
+ void test90degreeTurn() {
+ // This is a 90 degree turn about the point between front left and rear left wheels
+ // Module 0: speed 18.84955592153876 angle 90.0
+ // Module 1: speed 42.14888838624436 angle 26.565051177077986
+ // Module 2: speed 18.84955592153876 angle -90.0
+ // Module 3: speed 42.14888838624436 angle -26.565051177077986
+
+ final SwerveModuleState[] wheelSpeeds = {
+ new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
+ new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
+ new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
+ new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+ };
+ final var zero = new SwerveModuleState();
+
+ m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
+ final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+ assertAll(
+ () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01),
+ () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
+ () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
+ );
+ }
+
+ @Test
+ void testGyroAngleReset() {
+ var gyro = Rotation2d.fromDegrees(90.0);
+ var fieldAngle = Rotation2d.fromDegrees(0.0);
+ m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+ var state = new SwerveModuleState();
+ m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
+ state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
+ var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+
+ assertAll(
+ () -> assertEquals(1.0, pose.getTranslation().getX(), 0.1),
+ () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
+ () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+ );
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
new file mode 100644
index 0000000..dd33945
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.livewindow;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+class LiveWindowTest extends UtilityClassTest<LiveWindow> {
+ LiveWindowTest() {
+ super(LiveWindow.class);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
new file mode 100644
index 0000000..89ca3ec
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A mock sendable that marks itself as an actuator.
+ */
+public class MockActuatorSendable implements Sendable {
+ public MockActuatorSendable(String name) {
+ SendableRegistry.add(this, name);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setActuator(true);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
new file mode 100644
index 0000000..9824f11
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class ShuffleboardInstanceTest {
+ private NetworkTableInstance m_ntInstance;
+ private ShuffleboardInstance m_shuffleboardInstance;
+
+ @BeforeEach
+ void setupInstance() {
+ m_ntInstance = NetworkTableInstance.create();
+ m_shuffleboardInstance = new ShuffleboardInstance(m_ntInstance);
+ }
+
+ @AfterEach
+ void tearDownInstance() {
+ m_ntInstance.close();
+ }
+
+ @Test
+ void testPathFluent() {
+ NetworkTableEntry entry = m_shuffleboardInstance.getTab("Tab Title")
+ .getLayout("Layout Title", "List Layout")
+ .add("Data", "string")
+ .withWidget("Text View")
+ .getEntry();
+
+ assertAll(
+ () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
+ () -> assertEquals("/Shuffleboard/Tab Title/Layout Title/Data", entry.getName(),
+ "Entry path generated incorrectly"));
+ }
+
+ @Test
+ void testNestedLayoutsFluent() {
+ NetworkTableEntry entry = m_shuffleboardInstance.getTab("Tab")
+ .getLayout("First", "List")
+ .getLayout("Second", "List")
+ .getLayout("Third", "List")
+ .getLayout("Fourth", "List")
+ .add("Value", "string")
+ .getEntry();
+
+ assertAll(
+ () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
+ () -> assertEquals("/Shuffleboard/Tab/First/Second/Third/Fourth/Value", entry.getName(),
+ "Entry path generated incorrectly"));
+ }
+
+ @Test
+ void testNestedLayoutsOop() {
+ ShuffleboardTab tab = m_shuffleboardInstance.getTab("Tab");
+ ShuffleboardLayout first = tab.getLayout("First", "List");
+ ShuffleboardLayout second = first.getLayout("Second", "List");
+ ShuffleboardLayout third = second.getLayout("Third", "List");
+ ShuffleboardLayout fourth = third.getLayout("Fourth", "List");
+ SimpleWidget widget = fourth.add("Value", "string");
+ NetworkTableEntry entry = widget.getEntry();
+
+ assertAll(
+ () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
+ () -> assertEquals("/Shuffleboard/Tab/First/Second/Third/Fourth/Value", entry.getName(),
+ "Entry path generated incorrectly"));
+ }
+
+ @Test
+ void testLayoutTypeIsSet() {
+ String layoutType = "Type";
+ m_shuffleboardInstance.getTab("Tab")
+ .getLayout("Title", layoutType);
+ m_shuffleboardInstance.update();
+ NetworkTableEntry entry = m_ntInstance.getEntry(
+ "/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
+ assertEquals(layoutType, entry.getString("Not Set"), "Layout type not set");
+ }
+
+ @Test
+ void testNestedActuatorWidgetsAreDisabled() {
+ m_shuffleboardInstance.getTab("Tab")
+ .getLayout("Title", "Layout")
+ .add(new MockActuatorSendable("Actuator"));
+ NetworkTableEntry controllableEntry =
+ m_ntInstance.getEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
+
+ m_shuffleboardInstance.update();
+
+ // Note: we use the unsafe `getBoolean()` method because if the value is NOT a boolean, or if it
+ // is not present, then something has clearly gone very, very wrong
+ boolean controllable = controllableEntry.getValue().getBoolean();
+
+ // Sanity check
+ assertTrue(controllable, "The nested actuator widget should be enabled by default");
+ m_shuffleboardInstance.disableActuatorWidgets();
+ controllable = controllableEntry.getValue().getBoolean();
+ assertFalse(controllable, "The nested actuator widget should have been disabled");
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
new file mode 100644
index 0000000..1469170
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertSame;
+
+public class ShuffleboardTest extends UtilityClassTest<Shuffleboard> {
+ public ShuffleboardTest() {
+ super(Shuffleboard.class);
+ }
+
+ // Most relevant tests are in ShuffleboardTabTest
+
+ @Test
+ void testTabObjectsCached() {
+ ShuffleboardTab tab1 = Shuffleboard.getTab("testTabObjectsCached");
+ ShuffleboardTab tab2 = Shuffleboard.getTab("testTabObjectsCached");
+ assertSame(tab1, tab2, "Tab objects were not cached");
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
new file mode 100644
index 0000000..6e0da8e
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class SuppliedValueWidgetTest {
+ private NetworkTableInstance m_ntInstance;
+ private ShuffleboardInstance m_instance;
+
+ @BeforeEach
+ void setup() {
+ m_ntInstance = NetworkTableInstance.create();
+ m_instance = new ShuffleboardInstance(m_ntInstance);
+ }
+
+ @Test
+ void testAddString() {
+ AtomicInteger count = new AtomicInteger(0);
+ m_instance.getTab("Tab")
+ .addString("Title", () -> Integer.toString(count.incrementAndGet()));
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertEquals("1", entry.getString(null));
+
+ m_instance.update();
+ assertEquals("2", entry.getString(null));
+ }
+
+ @Test
+ void testAddDouble() {
+ AtomicInteger num = new AtomicInteger(0);
+ m_instance.getTab("Tab")
+ .addNumber("Title", num::incrementAndGet);
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertEquals(1, entry.getDouble(0));
+
+ m_instance.update();
+ assertEquals(2, entry.getDouble(0));
+ }
+
+ @Test
+ void testAddBoolean() {
+ boolean[] bool = {false};
+ m_instance.getTab("Tab")
+ .addBoolean("Title", () -> bool[0] = !bool[0]);
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertTrue(entry.getBoolean(false));
+
+ m_instance.update();
+ assertFalse(entry.getBoolean(true));
+ }
+
+ @Test
+ void testAddStringArray() {
+ String[] arr = {"foo", "bar"};
+ m_instance.getTab("Tab")
+ .addStringArray("Title", () -> arr);
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertArrayEquals(arr, entry.getStringArray(new String[0]));
+ }
+
+ @Test
+ void testAddDoubleArray() {
+ double[] arr = {0, 1};
+ m_instance.getTab("Tab")
+ .addDoubleArray("Title", () -> arr);
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertArrayEquals(arr, entry.getDoubleArray(new double[0]));
+ }
+
+ @Test
+ void testAddBooleanArray() {
+ boolean[] arr = {true, false};
+ m_instance.getTab("Tab")
+ .addBooleanArray("Title", () -> arr);
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertArrayEquals(arr, entry.getBooleanArray(new boolean[0]));
+ }
+
+ @Test
+ void testAddRawBytes() {
+ byte[] arr = {0, 1, 2, 3};
+ m_instance.getTab("Tab")
+ .addRaw("Title", () -> arr);
+ NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+ m_instance.update();
+ assertArrayEquals(arr, entry.getRaw(new byte[0]));
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
new file mode 100644
index 0000000..91210bc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.sim;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogInSim;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class AnalogInputSimTest {
+ static class DoubleStore {
+ public boolean m_wasTriggered;
+ public boolean m_wasCorrectType;
+ public double m_setValue0;
+ }
+
+ @Test
+ void setCallbackTest() {
+ HAL.initialize(500, 0);
+
+ AnalogInput input = new AnalogInput(5);
+ AnalogInSim inputSim = input.getSimObject();
+
+ for (double i = 0; i < 5.0; i += 0.1) {
+ inputSim.setVoltage(0);
+
+ assertEquals(input.getVoltage(), 0, 0.001);
+
+ inputSim.setVoltage(i);
+
+ assertEquals(input.getVoltage(), i, 0.001);
+
+ }
+
+ input.close();
+
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
new file mode 100644
index 0000000..7771425
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.sim;
+
+import edu.wpi.first.wpilibj.AnalogOutput;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogOutSim;
+import edu.wpi.first.hal.sim.CallbackStore;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class AnalogOutputSimTest {
+ static class DoubleStore {
+ public boolean m_wasTriggered;
+ public boolean m_wasCorrectType;
+ public double m_setValue = -1;
+
+ public void reset() {
+ m_wasCorrectType = false;
+ m_wasTriggered = false;
+ m_setValue = -1;
+ }
+ }
+
+ @Test
+ void setCallbackTest() {
+ HAL.initialize(500, 0);
+
+
+ AnalogOutput output = new AnalogOutput(0);
+ output.setVoltage(0.5);
+
+ AnalogOutSim outputSim = output.getSimObject();
+
+ DoubleStore store = new DoubleStore();
+
+ try (CallbackStore cb = outputSim.registerVoltageCallback((name, value) -> {
+ store.m_wasTriggered = true;
+ store.m_wasCorrectType = true;
+ store.m_setValue = value.getDouble();
+ }, false)) {
+ assertFalse(store.m_wasTriggered);
+
+ for (double i = 0.1; i < 5.0; i += 0.1) {
+ store.reset();
+
+ output.setVoltage(0);
+
+ assertTrue(store.m_wasTriggered);
+ assertEquals(store.m_setValue, 0, 0.001);
+
+ store.reset();
+
+ output.setVoltage(i);
+
+ assertTrue(store.m_wasTriggered);
+ assertEquals(store.m_setValue, i, 0.001);
+
+ }
+ }
+
+ output.close();
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
new file mode 100644
index 0000000..cc92ed0
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class SmartDashboardTest extends UtilityClassTest<SmartDashboard> {
+ private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("SmartDashboard");
+
+ SmartDashboardTest() {
+ super(SmartDashboard.class);
+ }
+
+ @BeforeEach
+ void beforeEach() {
+ m_table.getKeys().forEach(m_table::delete);
+ }
+
+ @Test
+ void getBadValueTest() {
+ assertEquals("Expected", SmartDashboard.getString("KEY_SHOULD_NOT_BE_FOUND", "Expected"));
+ }
+
+ @Test
+ void putStringTest() {
+ final String key = "putString";
+ final String value = "thisIsAValue";
+
+ SmartDashboard.putString(key, value);
+
+ assertEquals(value, m_table.getEntry(key).getString(""));
+ }
+
+ @Test
+ void getStringTest() {
+ final String key = "getString";
+ final String value = "thisIsAValue";
+
+ m_table.getEntry(key).setString(value);
+
+ assertEquals(value, SmartDashboard.getString(key, ""));
+ }
+
+ @Test
+ void putNumberTest() {
+ final String key = "PutNumber";
+ final int value = 2147483647;
+
+ SmartDashboard.putNumber(key, value);
+
+ assertEquals(value, m_table.getEntry(key).getNumber(0).intValue());
+ }
+
+ @Test
+ void getNumberTest() {
+ final String key = "GetNumber";
+ final int value = 2147483647;
+
+ m_table.getEntry(key).setNumber(value);
+
+ assertEquals(value, SmartDashboard.getNumber(key, 0), 0.01);
+ }
+
+ @Test
+ void putBooleanTest() {
+ final String key = "PutBoolean";
+ final boolean value = true;
+
+ SmartDashboard.putBoolean(key, value);
+
+ assertEquals(value, m_table.getEntry(key).getBoolean(!value));
+ }
+
+ @Test
+ void getBooleanTest() {
+ final String key = "GetBoolean";
+ final boolean value = true;
+
+ m_table.getEntry(key).setBoolean(value);
+
+ assertEquals(value, SmartDashboard.getBoolean(key, !value));
+ }
+
+ @Test
+ void testReplaceString() {
+ final String key = "testReplaceString";
+ final String valueNew = "newValue";
+
+ m_table.getEntry(key).setString("oldValue");
+ SmartDashboard.putString(key, valueNew);
+
+ assertEquals(valueNew, m_table.getEntry(key).getString(""));
+ }
+
+ @Test
+ void putStringNullKeyTest() {
+ assertThrows(NullPointerException.class,
+ () -> SmartDashboard.putString(null, "This should not work"));
+ }
+
+ @Test
+ void putStringNullValueTest() {
+ assertThrows(NullPointerException.class,
+ () -> SmartDashboard.putString("KEY_SHOULD_NOT_BE_STORED", null));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
new file mode 100644
index 0000000..45a0638
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+
+class CubicHermiteSplineTest {
+ private static final double kMaxDx = 0.127;
+ private static final double kMaxDy = 0.00127;
+ private static final double kMaxDtheta = 0.0872;
+
+ @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
+ private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
+ // Start the timer.
+ var start = System.nanoTime();
+
+ // Generate and parameterize the spline.
+ var controlVectors =
+ SplineHelper.getCubicControlVectorsFromWaypoints(a,
+ waypoints.toArray(new Translation2d[0]), b);
+ var splines
+ = SplineHelper.getCubicSplinesFromControlVectors(
+ controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
+
+ var poses = new ArrayList<PoseWithCurvature>();
+
+ poses.add(splines[0].getPoint(0.0));
+
+ for (var spline : splines) {
+ poses.addAll(SplineParameterizer.parameterize(spline));
+ }
+
+ // End the timer.
+ var end = System.nanoTime();
+
+ // Calculate the duration (used when benchmarking)
+ var durationMicroseconds = (end - start) / 1000.0;
+
+ for (int i = 0; i < poses.size() - 1; i++) {
+ var p0 = poses.get(i);
+ var p1 = poses.get(i + 1);
+
+ // Make sure the twist is under the tolerance defined by the Spline class.
+ var twist = p0.poseMeters.log(p1.poseMeters);
+ assertAll(
+ () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+ () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+ () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
+ );
+ }
+
+ // Check first point
+ assertAll(
+ () -> assertEquals(a.getTranslation().getX(),
+ poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+ () -> assertEquals(a.getTranslation().getY(),
+ poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+ () -> assertEquals(a.getRotation().getRadians(),
+ poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
+ );
+
+ // Check interior waypoints
+ boolean interiorsGood = true;
+ for (var waypoint : waypoints) {
+ boolean found = false;
+ for (var state : poses) {
+ if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
+ found = true;
+ }
+ }
+ interiorsGood &= found;
+ }
+
+ assertTrue(interiorsGood);
+
+ // Check last point
+ assertAll(
+ () -> assertEquals(b.getTranslation().getX(),
+ poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9),
+ () -> assertEquals(b.getTranslation().getY(),
+ poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9),
+ () -> assertEquals(b.getRotation().getRadians(),
+ poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
+ );
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testStraightLine() {
+ run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testSCurve() {
+ var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
+ ArrayList<Translation2d> waypoints = new ArrayList<>();
+ waypoints.add(new Translation2d(1, 1));
+ waypoints.add(new Translation2d(2, -1));
+ var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
+
+ run(start, waypoints, end);
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testOneInterior() {
+ var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+ ArrayList<Translation2d> waypoints = new ArrayList<>();
+ waypoints.add(new Translation2d(2.0, 0.0));
+ var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
+
+ run(start, waypoints, end);
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testWindyPath() {
+ final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+ final ArrayList<Translation2d> waypoints = new ArrayList<>();
+ waypoints.add(new Translation2d(0.5, 0.5));
+ waypoints.add(new Translation2d(0.5, 0.5));
+ waypoints.add(new Translation2d(1.0, 0.0));
+ waypoints.add(new Translation2d(1.5, 0.5));
+ waypoints.add(new Translation2d(2.0, 0.0));
+ waypoints.add(new Translation2d(2.5, 0.5));
+ final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
+
+ run(start, waypoints, end);
+ }
+
+ @Test
+ void testMalformed() {
+ assertThrows(MalformedSplineException.class, () -> run(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+ assertThrows(MalformedSplineException.class, () -> run(
+ new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+ Arrays.asList(new Translation2d(10, 10.5)),
+ new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
new file mode 100644
index 0000000..1d65922
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class QuinticHermiteSplineTest {
+ private static final double kMaxDx = 0.127;
+ private static final double kMaxDy = 0.00127;
+ private static final double kMaxDtheta = 0.0872;
+
+ @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
+ private void run(Pose2d a, Pose2d b) {
+ // Start the timer.
+ var start = System.nanoTime();
+
+ // Generate and parameterize the spline.
+ var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
+ var spline = SplineHelper.getQuinticSplinesFromControlVectors(
+ controlVectors.toArray(new Spline.ControlVector[0]))[0];
+ var poses = SplineParameterizer.parameterize(spline);
+
+ // End the timer.
+ var end = System.nanoTime();
+
+ // Calculate the duration (used when benchmarking)
+ var durationMicroseconds = (end - start) / 1000.0;
+
+ for (int i = 0; i < poses.size() - 1; i++) {
+ var p0 = poses.get(i);
+ var p1 = poses.get(i + 1);
+
+ // Make sure the twist is under the tolerance defined by the Spline class.
+ var twist = p0.poseMeters.log(p1.poseMeters);
+ assertAll(
+ () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+ () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+ () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
+ }
+
+ // Check first point
+ assertAll(
+ () -> assertEquals(
+ a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+ () -> assertEquals(
+ a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+ () -> assertEquals(
+ a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
+ 1E-9));
+
+ // Check last point
+ assertAll(
+ () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1)
+ .poseMeters.getTranslation().getX(), 1E-9),
+ () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1)
+ .poseMeters.getTranslation().getY(), 1E-9),
+ () -> assertEquals(b.getRotation().getRadians(),
+ poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testStraightLine() {
+ run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testSimpleSCurve() {
+ run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
+ }
+
+ @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+ @Test
+ void testSquiggly() {
+ run(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+ new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
+ }
+
+ @Test
+ void testMalformed() {
+ assertThrows(MalformedSplineException.class,
+ () -> run(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+ assertThrows(MalformedSplineException.class,
+ () -> run(
+ new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+ new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
new file mode 100644
index 0000000..977c5ab
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class CentripetalAccelerationConstraintTest {
+ @SuppressWarnings("LocalVariableName")
+ @Test
+ void testCentripetalAccelerationConstraint() {
+ double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
+ var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
+
+ Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+ Collections.singletonList(constraint));
+
+ var duration = trajectory.getTotalTimeSeconds();
+ var t = 0.0;
+ var dt = 0.02;
+
+ while (t < duration) {
+ var point = trajectory.sample(t);
+ var centripetalAcceleration
+ = Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
+
+ t += dt;
+ assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
+ }
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
new file mode 100644
index 0000000..d025860
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDriveKinematicsConstraintTest {
+ @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+ @Test
+ void testDifferentialDriveKinematicsConstraint() {
+ double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
+ var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
+ var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
+
+ Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+ Collections.singletonList(constraint));
+
+ var duration = trajectory.getTotalTimeSeconds();
+ var t = 0.0;
+ var dt = 0.02;
+
+ while (t < duration) {
+ var point = trajectory.sample(t);
+ var chassisSpeeds = new ChassisSpeeds(
+ point.velocityMetersPerSecond, 0,
+ point.velocityMetersPerSecond * point.curvatureRadPerMeter
+ );
+
+ var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+ t += dt;
+ assertAll(
+ () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
+ () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
+ );
+ }
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
new file mode 100644
index 0000000..f5c6ff3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDriveVoltageConstraintTest {
+ @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+ @Test
+ void testDifferentialDriveVoltageConstraint() {
+ // Pick an unreasonably large kA to ensure the constraint has to do some work
+ var feedforward = new SimpleMotorFeedforward(1, 1, 3);
+ var kinematics = new DifferentialDriveKinematics(0.5);
+ double maxVoltage = 10;
+ var constraint = new DifferentialDriveVoltageConstraint(feedforward,
+ kinematics,
+ maxVoltage);
+
+ Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+ Collections.singletonList(constraint));
+
+ var duration = trajectory.getTotalTimeSeconds();
+ var t = 0.0;
+ var dt = 0.02;
+
+ while (t < duration) {
+ var point = trajectory.sample(t);
+ var chassisSpeeds = new ChassisSpeeds(
+ point.velocityMetersPerSecond, 0,
+ point.velocityMetersPerSecond * point.curvatureRadPerMeter
+ );
+
+ var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+ t += dt;
+
+ // Not really a strictly-correct test as we're using the chassis accel instead of the
+ // wheel accel, but much easier than doing it "properly" and a reasonable check anyway
+ assertAll(
+ () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
+ point.accelerationMetersPerSecondSq)
+ <= maxVoltage + 0.05),
+ () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
+ point.accelerationMetersPerSecondSq)
+ >= -maxVoltage - 0.05),
+ () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
+ point.accelerationMetersPerSecondSq)
+ <= maxVoltage + 0.05),
+ () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
+ point.accelerationMetersPerSecondSq)
+ >= -maxVoltage - 0.05)
+ );
+ }
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
new file mode 100644
index 0000000..81fb5b4
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class TrajectoryGeneratorTest {
+ static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
+ final double maxVelocity = feetToMeters(12.0);
+ final double maxAccel = feetToMeters(12);
+
+ // 2018 cross scale auto waypoints.
+ var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
+ Rotation2d.fromDegrees(-180));
+ var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
+ Rotation2d.fromDegrees(-160));
+
+ var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(sideStart);
+ waypoints.add(sideStart.plus(
+ new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
+ new Rotation2d())));
+ waypoints.add(sideStart.plus(
+ new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
+ Rotation2d.fromDegrees(-90))));
+ waypoints.add(crossScale);
+
+ TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
+ .setReversed(true)
+ .addConstraints(constraints);
+
+ return TrajectoryGenerator.generateTrajectory(waypoints, config);
+ }
+
+ @Test
+ @SuppressWarnings("LocalVariableName")
+ void testGenerationAndConstraints() {
+ Trajectory trajectory = getTrajectory(new ArrayList<>());
+
+ double duration = trajectory.getTotalTimeSeconds();
+ double t = 0.0;
+ double dt = 0.02;
+
+ while (t < duration) {
+ var point = trajectory.sample(t);
+ t += dt;
+ assertAll(
+ () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
+ () -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
+ + 0.05)
+ );
+ }
+ }
+
+ @Test
+ void testMalformedTrajectory() {
+ var dsSim = new DriverStationSim();
+ dsSim.setSendError(false);
+
+ var traj =
+ TrajectoryGenerator.generateTrajectory(
+ Arrays.asList(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+ new Pose2d(1, 0, Rotation2d.fromDegrees(180))
+ ),
+ new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
+ );
+
+ assertEquals(traj.getStates().size(), 1);
+ assertEquals(traj.getTotalTimeSeconds(), 0);
+
+ dsSim.setSendError(true);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
new file mode 100644
index 0000000..e389a4f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class TrajectoryJsonTest {
+ @Test
+ void deserializeMatches() {
+ var config = List.of(new DifferentialDriveKinematicsConstraint(
+ new DifferentialDriveKinematics(20), 3));
+ var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
+
+ var deserialized =
+ assertDoesNotThrow(() ->
+ TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
+
+ assertEquals(trajectory.getStates(), deserialized.getStates());
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
new file mode 100644
index 0000000..19f31dc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class TrajectoryTransformTest {
+ @Test
+ void testTransformBy() {
+ var config = new TrajectoryConfig(3, 3);
+ var trajectory = TrajectoryGenerator.generateTrajectory(
+ new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
+ config
+ );
+
+ var transformedTrajectory = trajectory.transformBy(
+ new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
+
+ // Test initial pose.
+ assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
+ transformedTrajectory.sample(0).poseMeters);
+
+ testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+ }
+
+ @Test
+ void testRelativeTo() {
+ var config = new TrajectoryConfig(3, 3);
+ var trajectory = TrajectoryGenerator.generateTrajectory(
+ new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
+ List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
+ config
+ );
+
+ var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
+
+ // Test initial pose.
+ assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
+
+ testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+ }
+
+ void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
+ for (int i = 0; i < statesA.size() - 1; i++) {
+ var a1 = statesA.get(i).poseMeters;
+ var a2 = statesA.get(i + 1).poseMeters;
+
+ var b1 = statesB.get(i).poseMeters;
+ var b2 = statesB.get(i + 1).poseMeters;
+
+ assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
+ }
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
new file mode 100644
index 0000000..090eacc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
@@ -0,0 +1,257 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@SuppressWarnings({"PMD.TooManyMethods", "PMD.AvoidInstantiatingObjectsInLoops"})
+class TrapezoidProfileTest {
+ private static final double kDt = 0.01;
+
+ /**
+ * Asserts "val1" is less than or equal to "val2".
+ *
+ * @param val1 First operand in comparison.
+ * @param val2 Second operand in comparison.
+ */
+ private static void assertLessThanOrEquals(double val1, double val2) {
+ assertTrue(val1 <= val2, val1 + " is greater than " + val2);
+ }
+
+ /**
+ * Asserts "val1" is within "eps" of "val2".
+ *
+ * @param val1 First operand in comparison.
+ * @param val2 Second operand in comparison.
+ * @param eps Tolerance for whether values are near to each other.
+ */
+ private static void assertNear(double val1, double val2, double eps) {
+ assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
+ + " is greater than " + eps);
+ }
+
+ /**
+ * Asserts "val1" is less than or within "eps" of "val2".
+ *
+ * @param val1 First operand in comparison.
+ * @param val2 Second operand in comparison.
+ * @param eps Tolerance for whether values are near to each other.
+ */
+ private static void assertLessThanOrNear(double val1, double val2, double eps) {
+ if (val1 <= val2) {
+ assertLessThanOrEquals(val1, val2);
+ } else {
+ assertNear(val1, val2, eps);
+ }
+ }
+
+ @Test
+ void reachesGoal() {
+ TrapezoidProfile.Constraints constraints =
+ new TrapezoidProfile.Constraints(1.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
+ TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+ for (int i = 0; i < 450; ++i) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ }
+ assertEquals(state, goal);
+ }
+
+ // Tests that decreasing the maximum velocity in the middle when it is already
+ // moving faster than the new max is handled correctly
+ @Test
+ void posContinousUnderVelChange() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
+
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+ TrapezoidProfile.State state = profile.calculate(kDt);
+
+ double lastPos = state.position;
+ for (int i = 0; i < 1600; ++i) {
+ if (i == 400) {
+ constraints.maxVelocity = 0.75;
+ }
+
+ profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ double estimatedVel = (state.position - lastPos) / kDt;
+
+ if (i >= 400) {
+ // Since estimatedVel can have floating point rounding errors, we check
+ // whether value is less than or within an error delta of the new
+ // constraint.
+ assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
+
+ assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
+ }
+
+ lastPos = state.position;
+ }
+ assertEquals(state, goal);
+ }
+
+ // There is some somewhat tricky code for dealing with going backwards
+ @Test
+ void backwards() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+ TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+ for (int i = 0; i < 400; ++i) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ }
+ assertEquals(state, goal);
+ }
+
+ @Test
+ void switchGoalInMiddle() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+ TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+ for (int i = 0; i < 200; ++i) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ }
+ assertNotEquals(state, goal);
+
+ goal = new TrapezoidProfile.State(0.0, 0.0);
+ for (int i = 0; i < 550; ++i) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ }
+ assertEquals(state, goal);
+ }
+
+ // Checks to make sure that it hits top speed
+ @Test
+ void topSpeed() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
+ TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+ for (int i = 0; i < 200; ++i) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ }
+ assertNear(constraints.maxVelocity, state.velocity, 10e-5);
+
+ for (int i = 0; i < 2000; ++i) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ }
+ assertEquals(state, goal);
+ }
+
+ @Test
+ void timingToCurrent() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+ TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+ for (int i = 0; i < 400; i++) {
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
+ }
+ }
+
+ @Test
+ void timingToGoal() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+ TrapezoidProfile.State state = profile.calculate(kDt);
+
+ double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+ boolean reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ if (!reachedGoal && state.equals(goal)) {
+ // Expected value using for loop index is just an approximation since
+ // the time left in the profile doesn't increase linearly at the
+ // endpoints
+ assertNear(predictedTimeLeft, i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+ }
+
+ @Test
+ void timingBeforeGoal() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+ TrapezoidProfile.State state = profile.calculate(kDt);
+
+ double predictedTimeLeft = profile.timeLeftUntil(1);
+ boolean reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
+ assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+ reachedGoal = true;
+ }
+ }
+ }
+
+ @Test
+ void timingToNegativeGoal() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+ TrapezoidProfile.State state = profile.calculate(kDt);
+
+ double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+ boolean reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ if (!reachedGoal && state.equals(goal)) {
+ // Expected value using for loop index is just an approximation since
+ // the time left in the profile doesn't increase linearly at the
+ // endpoints
+ assertNear(predictedTimeLeft, i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+ }
+
+ @Test
+ void timingBeforeNegativeGoal() {
+ TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+ TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+ TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+ TrapezoidProfile.State state = profile.calculate(kDt);
+
+ double predictedTimeLeft = profile.timeLeftUntil(-1);
+ boolean reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = new TrapezoidProfile(constraints, goal, state);
+ state = profile.calculate(kDt);
+ if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
+ assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+ reachedGoal = true;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java
new file mode 100644
index 0000000..839e2e3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class ErrorMessagesTest extends UtilityClassTest<ErrorMessages> {
+ ErrorMessagesTest() {
+ super(ErrorMessages.class);
+ }
+
+ @Test
+ void requireNonNullParamNullTest() {
+ assertThrows(NullPointerException.class, () -> requireNonNullParam(null, "testParam",
+ "testMethod"));
+ }
+
+ @Test
+ void requireNonNullParamNotNullTest() {
+ assertDoesNotThrow(() -> requireNonNullParam("null", "testParam", "testMethod"));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
new file mode 100644
index 0000000..1d00046
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class UnitsTest extends UtilityClassTest<Units> {
+ UnitsTest() {
+ super(Units.class);
+ }
+
+ @Test
+ void metersToFeetTest() {
+ assertEquals(3.28, Units.metersToFeet(1), 1e-2);
+ }
+
+ @Test
+ void feetToMetersTest() {
+ assertEquals(0.30, Units.feetToMeters(1), 1e-2);
+ }
+
+ @Test
+ void metersToInchesTest() {
+ assertEquals(39.37, Units.metersToInches(1), 1e-2);
+ }
+
+ @Test
+ void inchesToMetersTest() {
+ assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
+ }
+
+ @Test
+ void degreesToRadiansTest() {
+ assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
+ }
+
+ @Test
+ void radiansToDegreesTest() {
+ assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
+ }
+
+ @Test
+ void rotationsPerMinuteToRadiansPerSecondTest() {
+ assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
+ }
+
+ @Test
+ void radiansPerSecondToRotationsPerMinute() {
+ assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
+ }
+}
diff --git a/wpilibj/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibj/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..f6d7b98
--- /dev/null
+++ b/wpilibj/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj.MockHardwareExtension
diff --git a/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini b/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
new file mode 100644
index 0000000..a685de0
--- /dev/null
+++ b/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
@@ -0,0 +1,7 @@
+[NetworkTables Storage 3.0]
+double "/Preferences/checkedValueInt"=2
+double "/Preferences/checkedValueDouble"=.2
+double "/Preferences/checkedValueFloat"=3.14
+double "/Preferences/checkedValueLong"=172
+string "/Preferences/checkedValueString"="Hello. How are you?"
+boolean "/Preferences/checkedValueBoolean"=false
diff --git a/wpilibj/wpilibj-config.cmake b/wpilibj/wpilibj-config.cmake
new file mode 100644
index 0000000..5e89f6d
--- /dev/null
+++ b/wpilibj/wpilibj-config.cmake
@@ -0,0 +1,2 @@
+get_filename_component(SELF_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+include(${SELF_DIR}/wpilibj.cmake)